NPS55-85-028 

NAVAL POSTGRADUATE SCHOOL 

Monterey, California 




TECHNICAL 



PROGRAM FOR THE SIMULTANEOUS ESTIMATION 
OF DISPLACEMENT AND ORIENTATION 
CORRECTIONS FOR SEVERAL SHORT 
BASE LINE ARRAYS 



ROBERT R. READ 

// 



NOVEMBER 1985 



Approved for public release; distribution unlimited. 
Prepared for: 

Naval Undersea Warfare Engineering Station 
Ceyport, WA 98345 

FedDocs 
D 208.14/2 
NPS-55-85-028 




_ - 0^2 



NAVAL POSTGRADUATE SCHOOL 
Monterey, California 



Rear Admiral R. H. Shumaker 
Superintendent 



D. A. Schrady 
Provost 



Reproduction of all or part of this report is authorized. 
This report was prepared by: 



security classification this page 



REPORT DOCUMENTATION PAGE DUDLEY KNOX LIBRARY 

lb. restrictive markingsNAVAL POSTGRAUUAI t SCHOOL" 
MONTEREY CA 93943-5101 



la. Rt-ORT SECURITY CLASSIFICATION 

UNCLASSIFIED 



2a. SECURITY CLASSIFICATION AUTHORITY 



2b. DECLASSIFICATION/ DOWNGRADING SCHEDULE 



3 DISTRIBUTION /AVAILABILITY OF REPORT 

Approved for public release; distribution 
unlimited. 



4 PERFORMING ORGANIZATION REPORT NUMBER(S) 

NPS55-85-028 



5. MONITORING ORGANIZATION REPORT NUM8£R(S) 



6a. NAME OF PERFORMING ORGANIZATION 

Naval Postgraduate School 



6b OFFICE SYMBOL 
(If applicable) 



7a. NAME OF MONITORING ORGANIZATION 

Naval Undersea Warfare Engineering Station 



6c. ADDRESS (Cry, Stare, and ZIP Code) 

Monterey, CA 93943-5100 



7b. ADDRESS (City, State, and ZIP Code) 

Keyport, WA 98345 



8a. NAME OF FUNDING / SPONSORING 
ORGANIZATION 



8b. OFFICE SYMBOL 
(If applicable) 



9. PROCUREMENT INSTRUMENT IDENTIFICATION NUMBER 



8c. ADDRESS (C/ry, Stare, and ZIP Code) 



10 SOURCE OF FUNDING NUMBERS 



PROGRAM 


PROJECT 


TASK^ 


WORK UNIT 


ELEMENT NO. 


NO 


NO. 


ACCESSION NO 



11 TITLE (incivae Secunry Classification) 

PROGRAM FOR THE SIMULTANEOUS ESTIMATION OF DISPLACEMENT AND ORIENTATION CORRECTIONS FOR 
SEVERAL SHORT BASE LINE ARRAYS 



2 PERSONAL AUTHOR(S) 

Read, Robert R. 



1 3a. TYPE OF REPORT 


13b TIME COVERED 


14 DATE OF REPORT (Year, Month, Day) 


15 PAGE COUNT 


Technical 


FROM TO 


1985 Novemher 


70 



16 SUPPLEMENTARY NOTATION 



17 


COSATI CODES 


18 SUBJECT TERMS (Continue on reverse if necessary and identify by block numoer) 


FIELD 


GROUP 


SUB-GROUP 


Least Squares, Calibration, Multivariate Estimation, 








Multivariate Regression 



19 ABSTRACT (Conf;nue on reverse if necessary and identify by block numoer) 



This report presents the mathematical 
estimation of multiple array displacement 
treated: (i) cross over data collected in 
first principal components straight lines 
the array overlap region. Fortran source 



support and algorithms for the least square 
and orientation corrections. Two situations are 
the array overlap region; and (ii) matching of 
for use when there is no replication of data in 
codes are provided for the first situation. 



20 DISTRIBUTION/ AVAILABILITY OF ABSTRACT 
&] UNCLASSIFIED/UNLIMITED □ SAME AS RPT 


□ DTIC USERS 


21 ABSTRACT SECURITY CLASSIFICATION 


22a NAME OF RESPONSIBLE INDIVIDUAL 

Robert R. Read 


22b TELEPHONE (include Area Code) 

(408)646-2382 


22c. OFFICE SYMBOL 

Code 55Re 



DO FORM 1473, 84 MAR 83 APR eon ion may be used until exnausted SECURITY CLASSIFICATION OF THIS PAGE 

All other editions are obsolete 



I . 



INTRODUCTION 



The methodology presented here is concerned with the cali- 
bration (more precisely, the maintenance of calibration) of a 
three-dimensional tracking system. The individual sensor arrays 
are the short base line arrays that produce 3-D track for NUWES . 

Our task is to consider the coherence of track produced by two 
arrays in the regions of array overlap. These regions make the 
continuous tracking of a target achievable. Thus many arrays may 
contribute to the production of track for a single target and the 
integration of the various track segments provided by the indi- 
vidual arrays is the main problem in the maintenance of calibration. 

An individual track segment produced by a single array is 
described originally in the local coordinate system of that array. 
Such a segment must be transformed to the Range coordinate system 
and integrated with other transformed segments to form a path for 
the target. These transformations are assumed to be linear and 
require two kinds of input; (i) the location of each array in the 
Range coordinate system; and (ii) the orientation of the local 
coordinate system relative to the Range orientation. In underwater 
tracking the location and orientation of a local coordinate sys- 
tem must be determined remotely. It is inferred from synchronously 
timed track segments. 

Each array has four sonar receivers located at four of the 
corners of a rigid cube. The target is a torpedo (or other 
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self-propelled vehicle) . It has a "pinger" attached to it which 
emits a sound wave at regularly timed intervals called point counts. 
The position of the target at a given point count is determined 
from the sound wave's time of arrival differential at the four 
corners containing the receivers. These signals are transmitted 
over cables to a central computer. 

The functioning of the Range's tracking system requires knowledgi 
of the location and orientation of each array. Moreover, once these 
values are established, they must be monitored regularly to guard 
against slippages of various forms, i.e., calibration must be 
maintained. 

There are a number of instrumentation, measurement and environ- 
mental problems associated with this type of system, but they are 
of no concern in the present work. Our goal is restricted to 
estimating array positions and orientations. Mathematically the 
former is a three-dimensional vector and the latter is a three by 
three coefficient matrix that is constrained to be orthonormal, 
that is, a rigid rotation of an array's cube in three space. 

This model for describing the changes in processing the local 
track from an array would be exact if the speed of sound in water we: 
constant. Such is not quite the case. This speed increases with 
depth and is assumed to be homogeneous in each horizontal layer of 
water. Because of the depth gradient there is a non-linear correc- 
tion term in the vertical position of the object. Generally it 
is not very large. If an array had sunk fifty feet deeper in 
a muddy bottom and a typical speed with depth gradient were used 
then sound ray tracing computations show that the change in 
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position of a target some 3000 feet away is within a foot of that 
computed using the linear model. For NUWES purposes it is safe 
to use the linear model to detect slippage. If the non-linear 
term were suspected of being important then the estimation 
methodology could be iterated. That is, the local coordinate 
system could be corrected using the linear methodology followed 
by a recomputation of the target's track. Then a reexamination 
of the consequences of this could be used to decide whether the 
local coordinate system should be corrected again. 

It will be seen that the corrections estimated for an array are 
not absolute but relative to the other arrays in the system. Be- 
cause of this it is convenient to assume that the location and 
orientation of at least one array, relative to the Range, were al- 
ready established. This fact has implications in the calibration 
maintenance problem. That is, the user places faith in the current 
official positioning of one or more arrays. Then possible changes 
in the positioning of others are estimated. If the user chooses 
to change his selection of this "anchor" array, the estimated 
changes for the others must be adjusted mathematically. 

The regions of overlap for two or more arrays will be called 
crossover regions. Crossover data refers to the tracks (in Range 
coordinates) in the crossover region which are produced by two or more 
arrays for a common set of point counts (i.e., time points). The 
estimation of array positioning changes is performed by attempting to 
make crossover data agree as much as possible in some global sense. 

We adopt the principle of least squares to serve in this role. 
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Experience with the least squares approach has revealed some 
occasional shortcomings. There exist cases for which the least 
square objective function is a rather flat surface as a function 
of the parameters that describe the changes. (This has occurred 
only when the number of arrays in a problem is two.) Since this 
can result in rather large estimated changes a second method, 
called the principal components method, has been developed. It 
involves the fitting of straight lines to the track segments in 
the crossover region, and this chosen array positions changes 
that will bring these lines together. It has worked well for the 
cases that cause trouble in the least squares approach. 

A brief comparison of the two methods follows : The principal 

components method does not require crossover data, i.e., two or 
more versions of track for a common set of point counts. It does 
require that the two segments of track being matched should be 
straight. Also, if the data are not literally of the crossover 
type, then one must also estimate the distance between the segments 
being aligned. The least squares approach requires crossover 
data, but allows the target to be maneuvering in the crossover regior 

The paper is organized as follows: The second section con- 

tains the development of the least squares method for the simplified 
case of two arrays producing track in a single crossover region. 

The main features of the method can be presented in this setting. 

In the third section the application of these ideas is extended 
to the general case of multiple arrays and a multiple set of cross- 
over regions. After that we develop the principal component 
method--again treating the two array problem first. 
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KEYMAIN, has been developed to produce 



A Fortran 77 program, 
the correction estimates from crossover data. The source codes 
for this program and all subroutines are contained in the Appendix. 
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II. MATHEMATICAL PRELIMINARIES 



In the formulas that follow z will always be a scalar, A and 
V column vectors, and B a square matrix. The quantities 3z/3C 
will have the same algebraic structure as C and the elements 
will be the partial derivatives of z with respect to the elements 
of C. The superscript T refers to matrix transpose. The 
following formulas are recorded: 



If 


z 


T 

= V A 


then 


3z/3A = V 


(2.1) 


If 


z 


T 

= A A 


then 


3z/3A = 2A 


(2.2) 


If 


z 


T 

= V BA 


then 


3z/3B = VA T 


(2.3) 


If 


z 


T T 

= V B V 


then 


3z/3B = 2BW T 


(2.4) 



Also the trace of a matrix (sum of the diagonal elements) will 
play a major role. If B and C are square matrices of the same 
order, liberal use will be made of the facts that 

Trace (BC) = Trace (CB) (2.5) 

and that the trace is a linear operator. The formulas (2.1) to 
(2.5) can be found in Reference 3, p. 7ff, and they are not 
difficult to develop directly. 

Consider a set of point counts S in a crossover region, and 
let the crossover data (in Range coordinates) be 



6 



y (t) 



for t e S 



1 




l 1 


J x l ( t)\ 


y(t) = 


Y2<t) ! 


s and x(t) = J 


Jx 2 (t) 


1 


1 Y 3 (t) > 


1 " 1 


k x 3 (t) ) 



provided by two different sensing arrays. Let us agree that the 

y(t) data comes from the array whose location and orientation 

are established and our goal is to check the calibration of the 

other array. In particular, does there exist a 3 vector A ^ 0, and a 

3x3 matrix B (^ I, the identity matrix) such that the adjusted track 
values , 

/\ 

x ( t ) = A + B X ( t ) , (2.6) 

are in better agreement with the y(t) than are the unmodified x(t). 

The vector A is related to a displacement of the sensing 
array and the matrix B is related to a correction of its orien- 
tation. If we let £(t) be x(t) in the local coordinate system, a be 
the location (in Range coordinates) of the array, and 6 the 
(orthonormal) orientation adjustment to the local coordinates then 

x ( t ) = a + BS(t) 



and 



x ( t ) = A + Ba + B6£ (t) (2.7) 

The corrected location and orientation adjustments are A+Ba and 
BB, respectively. 
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Our immediate goal is to estimate A and B using the principle 

of least squares. We will minimize the average square deviation 

between y(t) and A+Bx(t) for each point count. Using the squared 

2 T 

norm notation, ||V|| = V V, 

Q = Ave || y ( t ) -A - Bx ( t ) 1 1 (2.8) 

t 

and, since what follows involves a modification of the standard 
multivariate regression development, let us review an outline of 
the details. 

First, the estimator for A is 

/\ s\ 

A = y - B x (2.9) 

where y = Ave y(t) and x = Ave x(t) . The proof follows from an 
t t 

expansion of (2.8), namely 

Q = Ave[y(t) - Bx(t)] T [y(t) -Bx(t)] 
t ~ 

- 2 Ave[y(t) -Bx(t)] T A + A T A 
t ~ 

to which we apply the rules (2.1) and (2.2), 

= ~2 Ave [y ( t) - Bx ( t ) - A] . 

Set this equal to zero, solve for A and the result follows. 
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Next, let us make a change and work with the deviations 

Y (t) = y(t) - y and X(t) = x(t) - x (2.10) 

and use (2.9) in (2.8). The result is a simplified appearance 
for Q, 

Q = Ave || Y (t) - BX ( t ) || 2 (2.11) 

t 

Also introduce the covariance matrices 



D„ v = Ave (Y(t) Y T (t) } D = Ave {Y ( t ) X T ( t ) } 



yy 



yx 



D xx = Ave fe(t)X (t) } 



( 2 . 12 ) 



Now the customary estimator for B can be derived readily. Thus 



Q = Ave{Y T (t)Y(t) -2Y T (t)BX(t) + X T (t) B T BX ( t) } 
t 



(2.13) 



and use (2.3) and (2.4) to get 



= -2 Ave{Y (t)X T (t) - BX(t)X T (t) } 

3B t 



= -2{D - Bd 

yx xx 



(2.14) 



A -1 

using (2.12). Set this equal to zero and solve for B = D D 

which is the usual multivariate regression estimator, Ref. 1, p. 181. 
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Generally, the above usual estimator is not the one we should 
use. Recall that the sensing arrays are rigid cubes. If they 
have slipped (i.e., moved physically such as by the action of a 
ship's anchor hooking a cable) then they undergo a displacement 
and a re-orientation. The matrix B should be orthonormal as is 
the matrix 6 in the original calibration. Thus 

B T B = I (2.15) 



and the estimation of B involves the minimization of Q subject to 
the constraint. The usual estimate would be useful only if there 
were physical damage to an array, resulting in its behavior as a 
skewed rather than Cartesion system. 

Use of (2.15) and (2.12) in (2.13) allows the rewrite 



Q = Trace{D, r „ - 2D B + D } 
yy yx xx 



(2.16) 



and the minimization of Q is tantamount to the maximization of 



W = Trace{ D B T } subject to (2.15) (2.17) 

yx 

since the trace is a linear operator and B is involved only in 
the second term of (2.16) . The solution of the optimization 
problem (2.17) and its generalization in Section 4 is the bulk 
of our effort. 

Earlier was stated the need for placing faith in the location 
and orientation of at least one array, because otherwise the 
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solution could not be unique. Let us show now, using analysis, 
why this is the case in the two array, single crossover region 
problem. Suppose we tried to adjust both data sets, i.e., 



y ( t) = A 1 + B 1 y(t) and 

A 

x(t) = A 2 + B 2 x(t) 


(2.18) 


when both B^ and B 2 are constrained to be orthonormal. The 
squares objective function has the appearance 


least 


Q = Ave ||A 1 +B 1 y(t) - A 2 -B 2 x(t) || 2 


(2.19) 


It is clear from the development of equation (2.9) that the 
placement portion of the optimization can be estimated only 
relatively. That is, only A = A 2 - A^ can be found uniquely. 

A A A 

From the earlier development we can infer that A = B^y - B 2 x 
and re-express the objective as 


dis- 


Q(B 1 ,B 2 ) = Ave ||B 1 Y(t)-B 2 X(t) || 2 


(2.20) 



But this is the squared lengths of a set of vectors which have 
been averaged. Since the squared length of any vector is invari- 



ant under any orthonormal transformation, say C, it follows 


that 


q(b 1 ,b 2 ) = q(cb 1 ,cb 2 ) . 





T 

In particular we can take C = (i.e., the inverse of B^) and 

T 

then Q(B^,B^) = Q(I,B^). The product of orthonormal matrices 
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T 

is itself orthonormal . We can set B = B^^ and note that the 
minimization of Q(I,B) is that of (2.11), the problem we are 
treating. The geometrical interpretation is that the orientation 
of a sensing array can be matched only relatively to the other 
array. It is convenient to assume that the latter orientation 
is known . 
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III. SOLUTION ALGORITHM 



The matrix B has nine elements, but because of the constraint 
(2.15) there are only three degrees of freedom. That is, there 
are three length constraints and three orthogonality constraints. 
The three remaining degrees of freedom can be expressed in terms 
of the Euler angles. Ref. 2, p. 250 ff. 

Any orthnormal transformation in 3-space may be viewed as the 
application of three successive rotations, i.e., hold the x^-axis 
fixed and execute a rotation in the X 2 ~x^ plane; then hold the 
X 2 - axis fixed in its new position and rotate in the x^-x^ plane; 
and finally rotate in the x ]_“ x 2 plane with the x^-axis held fixed 
in its new position. Denote the angles of these three rotations 
as <|>^, <(> 2 » <J> 3 . In navigation work they are called roll, pitch 
and yaw. These angles are unique only when the order of applica- 
tion of the rotations is specified. 

To shorten the writing, let 

c^ = cos(cJk) and s^ = sinCcf^) (3.1) 

for i = 1,2,3, and the individual planar rotations can be expressed 
as 
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0 




and, using the order of application described, let 



B 




(3.3) 



We make liberal use of the fact that the product of orthonormal 
matrices is itself orthonormal. 

We note in passing that the matrices above are the trans- 
pose (and hence inverses) of those that are usually used in 
studying the motion of aeroplanes. Ref. [2]. Also the order of 
application is the reverse of the customary one thus making B in 
(3.3) the inverse of the usual matrix. This choice is appropriate 
for our work since the angular correction will rotate the local 
axes back to where they are expected to be. 

This done our optimization problem may be stated as 



The solution can be found by treating the problem as three 
successive two-dimensional problems. To do this let us first 
characterize how this optimization problem would be solved if 
the tracking took place in two dimensions. 

In the plane, an orthonormal matrix B has one degree of 
freedom and it is characterized by a rotation through an angle . 



Maximize Trace {D, 
^ 1 ' ^ 2 ' ^3 



yx 




(3.4) 
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As before, let c = cos(<j>), s = sin(<j>) so that 



B = 



c -s 



s c 



and 



W = Trace (DB ) = ( d i1 +D 22 )c + (D 21 -D 2 (s) (3.5) 

Thus W is itself a sine wave. It has one max and one min, and these are 
separated by ir radians. Since 



W' = -(D^ +D 22^ s + ^ D 21 -D 12^ c and 



W" = -(D 11 + D 22 )c - (D 21 -D 12 )s 



it is seen easily that the max occurs when 



s = K (° 2 i “ Dj_ 2 ) > c ~ K ^ D n +D 22^ ' and 



K = {(D 21" D 12 )2+ (D 11 +D 22 )2} 



- 1/2 



(3.6) 



This would be the solution if the tracking problem were a two- 
dimensional one. 

Now we are positioned to treat our three-dimensional tracking 
problem. Introduce matrices 



E = 



T T 
D yx p l p 2 



F = 



T t 

P-.D p. 
K 3 yx K l 



G = 



T T^ 

PoP->D 

2^3 yx 



(3.7) 
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and note that, using (2.5), (2.17) and (3.3), 



T 

W = Trace (D B } 
yx 

T 

= Trace(Fp^) 



T 

Trace {Ep 

T 

Trace(Gp^ } 



(3.8) 



These latter three representatives of W allow us to use our 
knowledge of the solution to the two-dimensional problem. When 
these three traces are written out in expanded form and compared 
with (3.5) and (3.6), it is seen that the optimal solution for 
(J)^, <j> 2 > $3 must have the form 



s 3 K 3 (E 21 E 12^ and c 3 K 3 (E 11 +E 22^ 



s 2 K 2 ^ F 31 F 13^ and C 2 K 2^ F H +F 33^ 



(3.9) 



S 1 K 1 (G 32 -G 23 ) and C 1 K 1 (G 22 +G 33 ) 



where 



K 3 ^ E 21 E 12 ^ + ^ E ll +E 22^ ^ 



- 1/2 



K 2 - ((F 31 - F 13 > 2 + < F 11 +F 33 )2) 



- 1/2 



(3.10) 



2 2 " 1/2 
K 1 " ^ G 32 G 23 ^ + ^ G 22 +G 33 ^ ^ 



16 



The above does not provide explicit solutions because 
E = E <t >2 ) ' F = F ^ 3 ) and G = G ( t $ 3 ) • It is necessary to 
get all three angles satisfying (3.9) and (3.10) at the same time. 
This means in addition that 



VW = 0 



(3.11) 



as well. We know that each of the equations 



3W 

3^3 



3W_ 

3*2 



3W 

3$! 



Trace 



Trace 



Trace 



3p: 



3<j> 



3p 



3<J). 



3p ; 
3^ 



0 



0 



0 



(3.12) 



has two solutions, one max and one min. It follows that (3.11) 

3 

has eight (2 ) solutions, one for each of the permutations of the 
component solutions, and this means one max, one min, and six 
saddle points. We require an algorithm that converges to the max. 

This can be achieved with an iterative process. Take an 
arbitrary beginning set of values for <j>^, ^ - AH <J> ^ = 0 

will do. Compute E for this choice and then compute a new 4>^ 
from the first member of (3.9). Use this value and the original 
<t>^ to compute F and from this compute a new <p^ from the second 
member of (3.9). Next use these new values for 4 * 2 ' ^3 an( ^ 
compute G, followed by a new 4>^ from the third member of (3.9). 
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This completes one cycle. At each step the appropriate member 
of (3.12) was satisfied and with a max. The value of W increased e< 
time. Moreover, a second cycle beginning with the values <p^ f ^ , and 4>. 
from the first cycle would produce a further increase in W. We 
know there exists a unique maximum for W and we have an iterative 
method that increases W at each step. It follows that, by 
repeating the process, we can come arbitrarily close to this 
maximum and that will be signaled when all three members of (3.12) 
are negligibly small in magnitude. 

In order to develop intuition that may be useful in under- 
standing the more complicated optimization process addressed in 
Section 4, let us take pause now and make a heuristic interpreta- 
tion of our iterative process. The objective function W is a 
sine wave as a function of each angle 4 k (with the other two 
angles held fixed) . As such it is concave in half of this restrict* 
angular domain. It follows that in the full three-dimensional 
space of (4> 1 , 4> 2 / 4> 3 ) the function W is concave in one eighth of its 
domain. A more general (and perhaps more rapidly converging) 
gradient search algorithm should begin in this part of the 
domain so that when convergence takes Diace, it is toward a 
maximum. If the algorithm presented is used, one does not have 
to worry about this point. 

We note in passing that the above technique is not limited 

to three dimensions. Suppose that our tracking problem was in 

p dimensions and that B was a p xp orthonormal matrix. This 

2 

constraint reduces the degrees of freedom, p in B by p for the 
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length constraints, and by an additional p(p-l)/2 for the 
orthogonality constraints. Since 

p 2 - p - p (p-1) /2 = p(p-l)/2 

we see that p(p-l)/2 is the number of angles (or equivalently, 
product matrices in the analogue of (3.2)) of rotation in B. 

I.e., it is the number of ways we can choose p-2 axes (from p 
axes) to be held fixed while a rotation takes place in the planes 
remaining. The solution structure will have one max, one min, 
and 2^-2 saddle points. Our iteration technique will converge 
to the max. 
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IV. GENERAL CASE 



To treat the general case we must consider several arrays, say 
K in number, and several crossover regions. Also we must allow for 
the target to be tracked in a given crossover region either not at 
all, exactly once, or more than once since it may maneuver back 
into a given region during a later point count set. Finally the 
target, during a given point count set in a given crossover region, 
may be tracked by more than one sensing array. We proceed to develo 
a notational structure that can handle this fully general case. 

Let S^,S 2 ,...,S^ represent a collection of R point count sets. 

It is convenient to assume that to each individual S s (for 
s = 1,...,R) there is associated a pair or more of sensing arrays. 
Thus, if only one array tracks the target in a particular crossover 
region there will be no corresponding point count set in the collec- 
tion. If exactly two arrays track the target then the particular 
S s is well defined. If three or more arrays track the target at 
about the same time, then things become a little fuzzy because the 
point count set for one pair of arrays may not be exactly congruent 
with the point count set of the crossover data of one of the two 
arrays with a third array, etc. This possible lack of congruence 
does not affect the computation of the cross covariance, (2.12) as 
they appear only in pairs. So there is no harm in allowing the sets 
Si,..., S r to duplicate some segments of track. When such duplicatio 
occurs however, the array pairs must be distinct. 

Next, for i = 1,...,K, let be the subcollection of 

th 

S 1# . . . ,S which has tracking data from the i — array. In this 
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way we can identify a 3-dimension vector of track data, X^(t,s), 

th 

as being produced by the i — array at point count t which belongs 

to S . These quantities exist only if S e C-. 

s si 

Our goal is to estimate the displacement and re-orientation 
parameter pairs for each of the K arrays. Earlier, with K = 2, 
we learned that there is an identif iability problem and it was 
convenient to assume that the location and orientation of one 
of the arrays was fixed. The same is true in the general case 
(i.e., only one array is fixed) provided that the data satisfy a 
connectivity condition. This condition can be described in the 
parlance of graph theory. The K sensing arrays form the nodes. 

An arc exists between two nodes if crossover data exists between 
them. We wish to consider only connected graphs. What this 
means is: There is no partition of the nodes into two non- 

empty sets such that an arc cannot be found connecting a node of 
one set to a node of the other set. 

If this condition of connectedness is not satisfied by our 
problem, then the overall data must be decomposed into smaller 
sets so that it does hold for each subset. Such decompositions 
are unique and each member of the decomposition is to be treated 
separately. This done we can fix our analysis on the case that 
involves a connected graph. Here it will be seen that the estima- 
tion of all displacements and re-orientation parameters will be 
unique once that one set (i.e., a set corresponding to a given 
array) is specified. 

Our notation needs to be expanded. Let be the 3- 

. th 

dimensional track, in range coordinates, produced by the 1 
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array at point count t belonging to S . Our goal is to estimate 
the location vectors A^ and re-orientation matrices so that 
(compare (2.6) ) 

A 

x^tjs) = A i + B i x i (t;s) (4.1) 

are as compatible as possible with the overall track of the 
target in the range. Also let x^(s) be the vector of averages 
of the x^(t;s) taken over t in S s , and 

X i (t;s) = x^(t;s) - x^(s) (4.2) 

be the deviations from the mean. Our new objective function can 
be expressed as an extension of (2.19) 

Q = III N Ave || B. X . (t;s) - B .X . (t; s) 1 1 (4.3) 

i< j s s 11 3 11 

where the outer double sum is over 1 < i < j < k, the inner sum 

is over s e C . nC . , and the average is to be taken over t e S . 

i 3 s 

The weights N g = number of point counts in S g for the array 
pair (i, j) . 

We can view (4.3) as 

Q = Q(B 1 ,B 2 , . . . ,B k ) (4.4) 

and, because of the connectedness assumption, (4.3) cannot be 
decomposed into the sum of two terms 
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Q -I ( B . , • • • / B . ) 3 nd Q«(B. , • • . , B . ) 

1 1 1 X p 3 1 D r 

for which the subscript sets i.,...,i and j n , ...,j form a 

1 p 1 r 

partition of 1,2,...,K. Now we can reformat the argument used 
at the end of Section 2. If C is any orthonormal transformation 
then Q (CB^ , CB 2 , . • . ,CB R ) = Q (B^ , . . . ,B R ) . Using C = B^, say, then 

Q(B 1 ,... / B r ) = Q(I,B^B 2 , . . . ,B^B k ) (4.5) 



and, for convenience, it is assumed that the orientation of the 
first array is known, i.e., B^ = I in (4.3). 

Define covariance matrices 



D i j(s) = Ave{X^ (t; s) Xj (t; s) } 



(4.6) 



where the average is taken over the point counts t in S g . 

These quantities exist only for s e C^nCj i- 0. Then (4.3) may 
be expanded to the more useful form 



Q = Trace 



l l l 

i < j s 



N { D . . ( s ) - 2D . • ( s ) bTb + D . • ( s) } 
s H iD j 1 JJ 



(4.7) 



and again, because the unknown matrices {B^} appear only in the second 
term, we may choose to maximize 



W = 




(4.8) 
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for 1 £ i < j £ K and 

D. • = 7 N D. . (s) or zero (4.9) 

ID 3 s id 

according to whether the summation for s e nC^ has content, 
or is empty. 

A word about the computation of D^j(s) in (4.9). It seems 
wise to use the pooled within groups covariance in those cases 
for which S s is the union of rather diverse point counts. An 
example should make the point. Suppose the array pair (i,j) 
tracks the target at point counts {l,...,25} and also {86 , . . . , 135 } . 
The pooled within groups covariance would compute the two covari- 
ances separately and then combine them into one using a weighted 
(by sample size) average. In our example, the weights would be 
25/75 and 40/75. 

For each fixed value of r (r = 1 , . . . , K) the right hand side 
of (4.8) contains an expression of the form 

W = I Trace{D. B T B . } + 7 Trace{D .B T B } 

r i< r ir r 1 j > r ^ r 

= Trace{ [ B.D. B^ + l D • bTb } (4.10) 

i<r j>r J J 

To shorten the writing let 



i<r 



B.D. 
l ir 



and 



j >r 



T 

D .B 
rj D 



(4.11) 
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T T T T 

and use the fact that TracelF^^} = TraceiB^F^} = TracetF^B^,} . 

Then 

W r = Trace{ (E r + F^)B^} (4.12) 

Also it is interesting to note that each term of (4.8) appears 
in two and only two distinct W . It follows that 

k 

I W r = 2W (4.13) 

1 r 

Now the vector of partial derivatives of W with respect to the 
three Euler angles in B r , that is <J> r ^, <f > r2 / <£ 3 / is the same as 
the gradient of W r (with B-^ , . . . ,B r-1 ,B^ + ^ , . . . ,B^ held fixed). 
Moreover, the structure of (4.12) is the same as that of (3.4). 

We know from Section 2 that VW r has eight zeros, exactly one of 
which corresponds to a max, and we have an algorithm that converges 
to that max. 

The analysis above leads to the construction of a gradient 

search that can ferret out the max of W. To fix one array we set 

B^ = I and keep this throughout. Choose starting matrices for 

the set B~,...,E . Compute E = E (B. , . . . ,B ) and 
^ K 3T 1C X 2T -L 

F r = F r (B r+l' ’ " ,B K ] for each r = 1 / 2 / • • •, K (E x = 0 E F r ) . Use 
the algorithm in Section 2 applied to (4.12), for each r, to 
produce the new Euler angles, i.e., the system {B r }. Use these 
to recompute the { E r ' F r } an< ^ repeat. Stop when the gradients 
of all W r are sufficiently small in magnitude. Each W will 
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be at a locaJ maximum for its argument B r with all of the other 
orientation matrices held fixed. 

There are some open questions about the convergence proper- 
ties of this algorithm. From an empirical point of view it has 
appeared to work successfully. Convergence is not monotone how- 
ever. In none of our test problems have the {B^} departed notably 
from identity matrices, and the use of all B. = I to initialize 
the algorithm has provided satisfactory results. For one run 
the initialization was chosen at random (in the 3 (K-l) dimensional 
space of Euler angles) and the time to convergence was excessive. 
We know that W has many saddle points and that it is a concave 
function in only a small part of its domain. 

Methods for initializing the algorithm need to be developed. 
The following idea may hold some promise: Referring to each 

term of (4.8), 



TraceiD . .B T b . } = Trace{D. .B . T} , 

13 3 1 i] i] 

T 

where B* ^ = B^^ , the solution algorithm of Section 3 can be 
used to check whether B^ is near to the identity. If so, 
initialize all B^ = I. If not, then study of the resulting {B|^ } 
may lead to a good selection. 

Let us return to the objective function Q of (4.3) and use 
(4.1) in it for purposes of estimating the location parameters 
{ A r } . Formally we have 



Q 



l l l N s Ave 

i< j s 



A i 



+ B 1 x i (t ; s) - 



A. - B 






( t ; S ) 



(4.14) 
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where 1 < i < j <_ k; s e nC^ ; the average is taken over the 

points counts t in S ; and N = number of point counts in S . 

s s s 

Again it is useful to use terms 



Q = y y N Ave IIa.+B.-X. (t;s) - A -B X. (t;s) 
r ■ L s 11 1 r r-i 



i<r s 



+ l \ N Ave |I A +B x (t;s) -A. -B.x. (t;s) 
j <r s ~ J J~J 



(4.15) 



K 

and recognize that \ Q = 2Q. In a manner similar to the one 

r=l 

that produced (2.7) we can develop 




-2 l l N s {B i x i (s) -B r x r (s) - (A -A i ) } 
i<r s 



+ 2 



y 7 N {B x (s) -B.x. ( s) - (A. -A )} 
j>r s s r ~ r 3 r' 



(4.16) 



Setting (4.16) equal to zero results in a 3. K linear system in 

the A, , . . . , A T ,. Let M. . = y N for s e C . n C ■ . Then the left 
1 K ll u s l J 

s 

hand side of the linear system is 



- y m. a. +{ y m. + y m . >a 

• L ir i lr jL ^ 



i<r 



i<r ]>r 



and the right hand side is 



l 

j >r 



M .A. 
3 



(4.17) 
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y b. y n x. s ) + y b. 

. L 1 L s - 1 . L 1 

i<r s ]>r J 



N s Xj( s ) 



- V.I l N s ^ r (s) + l l N s x r (s) } 



i<r s 



j >r s 



(4.18) 



Notice that the coefficient matrix in (4.17) is the same for all 
three components of the {A r >. Also the columns of the matrix 
add to zero so that its rank is K-l. It follows that the system 
is underdetermined. We anticipated this. Setting A^ = 0 will 
allow a unique solution for / • • • f A Of course, this choice 
corresponds to the assumption made earlier that the location and 
orientation of the first array is known. 
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V. PRINCIPAL COMPONENTS METHOD 



Experience with the least square method (two arrays. Section 
2) in the area of underwater tracking has revealed some instabili- 
ties. Several cases were identified in which the surface W con- 
tained some rather extensive relatively flat regions as a function 
of the three Euler angles. More specifically, it is possible to 
move from the maximizing point to a saddle point and lose less 
than one percent of the value of W. Moreover the effect of using 
the maximizing point can be to assign an absurd relocation geometry 
to the position of the sensor (e.g., turned upside down and sus- 
pended without support somewhere in, or above, the water). In 
these cases use of the saddle point involved only minor relocation 
of the sensor. Clearly this is an unsatisfactory state of affairs. 

These anomalies can happen, and there is not yet any fully 
reliable way to identify the conditions under which they may 
occur. They seem to be related to the inherent variability of 
the tracking data coupled with the point by point crossover data 
matching approach utilized by the least square methodology. It 
may be that certain naturally occurring geometric conditions 
(e.g., straight line structure of track) may act as a catalyst 
for this phenomenon. 

An alternative methodology has been developed and it appears 
to avoid producing the absurd solutions mentioned above. It is 
based on the matching of the first principal components of the 
covariance matrices of the tracking data. 
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Let us take a moment to describe the notion of first princi- 



pal component. Suppose we want to fit a segment of straight 
line to a track of data {x(t);t eS}. Further suppose we use the 
minimum sum of squared projections (orthogonal) as the mathemati- 
cal criterion for choosing the line. The solution is well known 
(Ref. 1, p. 272 ff) . Let A be the largest eigenvalue of the 

covariance matrix D and let q be an eigenvector associated with 

xx 

A. That is, any solution of 

D xx q = Xq (5 * 1) 



The following facts are stated without proof: 

(i) The vector q is a set of direction numbers for the 
desired straight line. 

(ii) The line passes through the centroid x. 

(iii) The projected track of x(t) onto this straight line 

T 

are the scalar values u(t) = q x(t). 

These properties can be exploited for our current problem in 
the following way. Let p be a first principal component for D 
and q play the same role for D . Both are assumed to be scaled 

XX 

to have length one: 




l 



2 

q i 



Adopt as the optimizing criterion: Choose B orthonormal so that 

P = Bq (5.2) 
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That is, the re-orientation matrix is chosen so that the two 
fitted straight line segments have the same direction numbers. 

It appears that the mathematical problem, p = Bq, is most 
easily solved using a constructive method. If p = q then B = I. 
Barring this, proceed as follows: 

1) Let 6 be the angle between p and q. 

2) Form an orthonormal basis H = such that h^ = q; 

h 2 is in the plane of p and q but orthogonal to 

h^ completes the basis. 

/ 2 

3) Letting c = cos (0) and s = vl -c , we can use the Gram 
Schmidt process and take 

h . <q -c P ) 

2 s 

4) Notice that 




and that Hp can be rotated through an (Eulerian) angle 

T 

e into Hq by applying the matrix p^O), see (3.2). 

5) Since p^Hp = Hq and both p^ and H are orthonormal matrices, 

T T 

it follows that p = H p.jHq. Thus 



B 



T T 
H P 3 H 



(5. 3) 



Notice that this method does not require crossover data in 

the strict sense, i.e., matched pairs of track from two arrays 

for a common set of point counts. The matrix D is not needed 

yx 
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and D and D^ x can be computed without this requirement. 
However the stability of the principal components p and q 



require that the two pieces of track be rather straight, i.e., 
no curvilinear bias. 

If the input data are not crossover data, there must be an 
adjustment in the way that we estimate the displacement parameter 
A. The formula (2.9) will not be valid because the x and y 
values do not refer to the same time (i.e., point count). Our 
immediate goal is to estimate the distance 6 traveled by the 
target from its mean position y at some time t to its new esti- 
mated position A + Bx at some later time t . Let us assume that 
the two point count sets (one for the {y} and one for the {x}) 
are mutually exclusive and represent equally spaced time values. 
Since the target's path is straight we can average the times in 

the two point count sets to obtain values for t and t . Then 

y x 

one can form the sets of projections 



u ( t ) = q x ( t ) 



and 



v ( t ) = p y (t) 



(5.4) 



and use their successive differences to get an estimate of the 
target's speed. 

The distance 6 will be the speed multiplied by the time 

increment t - t . 

x y 

This done, it is claimed that 



A = y - Bx + 6p 



(5.5) 
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Proof: The lineal distance 6 can be represented as 

T — — 

p (A+Bx-y) = 6 

T T — — 

from which it follows that p A = p (y - Bx) +6. We can view p 

as the first column of a seld adjoint matrix P, and letting 
T 

6 = (6,0,0) we can express our equation in the form 

P T A = P T (y-Bx) + 6 (5.6) 

From (5.6) it follows that A = (y - Bx) + P6 and this is the 
same as (5.5). 

It should be noted that the principal components method could 

have been used in place of the least squares approach. Having 

crossover data in hand does not preclude its use. Thus t = t 

Y x 

and 6 = 0 in (5.5) . We can modify the objective function Q of 
(2.11) and get a perfect fit, i.e., because of (5.2), 

0 = ||p- Bq j | 2 = P T P - 2p T Bq + q T q 

= 2-2 Trace {pq T B T } (5.7) 

and the Trace factor of (5.7) is unity. 

With the above in mind let us turn to the question of extend- 
ing the principal components method to several arrays and 
several crossover regions. It is simpler if we have crossover 
data so let's describe that situation first. 
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The notation is the same as in Section 4. Let q i (s) be 
the first principal component of D^^(s) (see (4.6)). The 
orientation changes {B^} will be selected first by minimizing 



Since this has the same structure as (4.8), the same solution 
algorithm can be used. Also the system (4.17) and (4.18) can be 
used to obtain the {A^}. 

The procedure changes a bit when the data are not of the 
crossover form. That is, when we are splicing together segments of 
track and there are no overlaps. One might liken this problem 
as being similar to putting in water pipes in a large building. 
Several subcontractors have put in their pipes and our job is to 
connect it all up. The pieces must be moved so that the ends 
butt up against one another. 

With this view, we need a change in terminology. Let 
Si'..., S r represent the crossover points, or connection points. 

To each S g there corresponds two arrays, i.e., an unique (i,j) 



Q 




l l £ {2 - 2 Trace [q^ ( s ) qT ( s ) bTb . ] } 

i ^ -i e J J 



(5.8) 



and the summations are for 1 <_ i < j <_ K and s e nCj . As 
before, this is tantamount to maximizing 



W 



i<j 



l I Trace{D ij B^B i } 



ID D i 



with 



s 



l q i (s)qT(s) (5.9) 
C -J 
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pair. Let T^(s) and Tj (s) be the point counts that produce the 
data. It is assumed that they do not overlap. Each is suffi- 
ciently extensive so as to provide stable estimates for the 
covariance matrices D^^(s) and Djj(s), yet not so extensive as 
to compromise the assumption that the target is moving in a 
straight line while in the point counts neighboring the connec- 
tion. This done, we can input equation (5.9) and use the algorithm 
of Section 4 to estimate the {B^}. 

It remains to estimate the {A^}. To do this the technique 
preceeding equation (5.5) needs to be expanded. With more than 
two arrays involved, the objective function Q of (5.8) will not 
be zero. This means that our reoriented segments of track will 
not share an exact common line at the connection points. With 
this possibility in mind we introduce some further quantities. 

To each of the sets T^(s) and Tj (s) adjoin (if necessary) a 
common time value (or pseudo point count) t^j(s) which will serve 
as the connecting time. Let t^(s) and tj (s) be the average 
values of T^(s) and Tj (s) respectively. For convenience it is 
assumed that the former preceeds the latter; t^(s) < tj (s) . The 
corresponding pre-adjusted positives are x^(s) and Xj (s) . Esti- 
mate the target's speed in the same array as before and use it 
to compute the distances: 



6^(s) = distance traveled from t^(s) to t^j ( s ) 



6 j (s) = distance traveled from t^j (s) to tj(s) 
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These will be used in our immediate goal, namely, to take the 
linearly adjusted pieces of track 



x^(t;s) = A^ + (t ; s ) 

(5.10) 

/\ 

Xj (t ; s) = Aj + Bj Xj ( t ; s ) 



and model the differences Aj - A^ in terms of the 6^(s), <5j(s) 
and other known quantities. 

In continuing, let us drop the argument s from the notation. 
Again we can use the eigenvectors, , to project the track to 
an axis so we can measure the distances. Specifically, write 



q i x i (t ii> ‘ 9i tA i +B i x i J 



q j |A j + Vj' - q j x j (t ij ) 



5 i 



(5.11) 



= 6 



Although we don't know how to compute x^(t^j) and Xj(t^j) they 
both represent the position at the connecting point, 

A A 

x i(tij) = Xj (t ). Next rewrite (5.11) as 



T a 

q . A • 
D 



T 

q . A . 

i 



q j x i (t ij» 



q i x i {t ij > 



T — 

q .B . x . + 6 • 
ODD 3 



T — 

q B.x. - 6. 

^lil l 



(5.12) 



Then apply the same technique as used in the proof of (5.5). 
The result is 
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(5.13) 



A j = 


A 

x • ( t . . ) - B • x • + q . 6 . 

3 ID 3 3 j D 

/ C 1 Q \ 


A i - 


\ ^ • J- ^ / 

A 

x- (t . • ) - B . x- — q . 6 • 

l lj li ^i i 




A 



The unknown quantities x^(t^j) will cancel when we take 



differences. Let 


us restore the s to the notation and record 



the result. 



- 


Bi x i ( s ) “ Bj x j (s) + <$j(s)qj(s) + 6 i (s)q i (s) 

(5.14) 


The system (5.14) 


is overdetermined and has no solution. Let us 



develop the least squares compromise. To shorten the writing let 
g^j (s) represent the right hand side of (5.14). Many details 
will be omitted because we follow the pattern at the end of 
Section 4. 

Let 



Q = 


I I l II A i - A; -g,. (s) || 2 (5.15) 

i<j s J J 



and 



o 

II 

H'OO 

A 

h o~u 
(fl 0-0 

> 


' A i - g ii (s) II 2 + l l I ||A. -A r -g r (s) || 2 
J j>r s J J 


Then 
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(5.16) 




l l HA r -A, 



-g 



ir 



(s) } 



- I l I(a -a -g (s)} 

j>r s J J 



which is set equal to zero for each r = 1 , . . . , K. 
the number of (i,j) connections that exist, i.e., 
and proceed. For r = 1,...,K we have the system 



Let n^ be 



n 



il 



l 



- I 

i<r 



n ir A i 



A { 
r 



n 



i<r 



ir 



l 

j>r 



n 



rj 



~ I 

j>r 



n • A . 
rj 3 



= I I 9 ir (s) " l I 9 ri (s) 

i<r s j>r s J 



(5.17) 



and the gj_ r ( s ) a ^d g r j (s) can be found from the right side of 
(5.14). As before, the columns of the coefficient matrix add to 
zero and the rank of the system is K-l. We set = 0 (i.e., 

the location of the first array is assumed known) and solve. 
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APPENDIX 
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SUBROUTINE CALL CHART 




* * 
* 
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called at two different places in the calling subroutine 

CALLED AT FIVE DIFFERENT PLACES IN THE CALLING SUBROUTINE 




oo o ooooo o o o oo oo ooooooooooooo 



PROGRAM KEYMAIN 

#****####***#*##*#####*#*#*******#*****###*****##*******##****} 
*** This serves as a main program to call the subroutines *** 
*** used to estimate the sensor array displacements and *** 
*** the array re-orientation angles for the Keyport range * * 
*** calibration project. It reads in data from a disk **’ 

*** file specified by the user. It prompts the user for *** 
*** this data and for sensor array I.D. information. 

*** -- 26 August 1985 **’ 

*####*#####****#########**##******####*##***####***#*******#**} 



... Declare all integer variables: 

INTEGERS IND2 ( 2 , 3 0 ) , INDI (30,30) , Rl, NUMREC, I, J, IDL, 

2 K, IA ( 3 0 ) , TESTC, NS1(30), IND(30,30), R, NS (30 ) » 

3 FF(30,30), IK, KM, IDR, IND2R(2,30), DATSET (30), 

4 CHOICE, OUT, IN 

... Declare all real variables in double precision: 

RE AL *8 AA ( 200 , 6 ) , CROSSA ( 3 0 , 3 , 3 ) , MEAN(30,6), DEV(S0,3,3), 

2 XB ( 3 0 , 6 ) , EP, 6(30,3,3), BB(3,3), XBB(30,6), A(30,3), 

3 DEL ( 3 0 , 3 ) , D ( 3 0 ) , POO), COO), RM(6), PP, EA(3C,3) 

... Declare the character variables: 

CHARACTER DSNAME*13, ANSWER*3 

LOGICAL EXST 

PARAMETER (0UT=6, IN=5) 

WRITE(0UT,*) ' This is a user friendly program. Hello !!. f 
WRITE (OUT,*) * « 

Rl = 0 

... The IND2 and IND2R matrices serve to store array pciir 

identification information and relate it to the Input 
(six column) crossover data sets. 

... Initialize IND2 and IND2R matrices to zero: 

DO 30 I = 1,2 
DO 30 J = 1,30 
I N D 2 ( I , J ) = 0 
I N D 2 F; ( I , J ) = 0 
30 CONTINUE 

5 V» RITE (OUT,*) ' Please enter the name of the data set on disk: • 

... Rl counts the number of data sets input by tiie user. 

Rl = Rl + 1 
C 
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o o o o o o o o o o o o o o o 



READ( IN, » (A) ' ) DSNAME 

C ... Check to make sure data file exists: 

INQUIRE ( FILE= DSNAME ,EXIST=EXST) 

IF ( . NOT. EXST) THEN 

WRITE ( OUT, *) ' The file does not exist.’ 

9 WRITE ( OUT, *) ’ Do you want to (1) try again or (2) abort?’ 
READ ( IN, * ) CHOICE 

IF (CHOICE .EQ. 1) THEN 
R1 = R1 - 1 
GOTO 5 

ELSE IF (CHOICE .EQ. 2) THEN 

WRITE ( OUT, *) ' Program terminating at your request.' 
STOP 

ELSE IF ( CHOICE .NE. 1 .AND. CHOICE .NE. 2) THEN 
WR ITE ( OUT, * ) ’ Please enter integer 1 or 2 
GOTO 9 
END IF 
END IF 
C 
C 

OPEN ( 1, FILE* DSNAME, STATU S= * OLD' ) 

... Read data from file and count number of records (NUMREC): 
NUMREC = 0 

10 NUMREC = NUMREC + 1 
READ(1,*,END=20,ERR=15) ( AA ( NUMREC , J ), J = 1 , 6 ) 

GOTO 10 

... Notify operator of file read error: 

15 WRITE (OUT, *) ’ ’ 

W R I T E ( 0 U T , * ) ' An error was detected in reading the file,' 
WRITE ( OUT, *) ' but execution will continue anyway ...' 

WRITE (OUT, *) » ' 

WRITE ( OUT » * ) ' ' 

20 NUMREC = NUMREC - 1 

WRITE(OUT,*) ' There are ', NUMREC,' records i r. date set '»D 
CLOSE ( UN IT= 1 ) 

... The number of records per input data set are accumulated 
in the vector NS1. 

NSl(Rl) = NUMREC 



... Define left <L right sensor no.'s for matrix 1 N D 2 : 

"left" will be indexed with a one and "ri^ht" will 
be indexed with a two. 

WRITE ( CUT, * ) ' Marne your left sensor array number: ' 

READ( IN, *) IDL 

W R ITE ( OUT, * ) ' Name your right sensor array number: ' 

READ (IN,*) I DR 

... The left and right array ID's ore entered in the 
first anti second (respectively) rows of IHD2 . 

I N D 2 ( 1 , R 1 ) = IDL 

IND2 ( 2, R1 ) = IDR 
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... Begin subroutine calls: 

Subroutine CROSSP takes each input data set AA, which 
has NUMREC records, and returns RM -the six component t 
vector and C-the three by three matrix of sums of cros: 
deviations from the means. These are computed sequent 
and stored in the arrays MEAN and CROSSA as they are c< 

CALL CROSSP(NUMREC»AA,RM,C) 

DO 50 I = 1,3 
DO 50 J = 1,3 
CROSS A ( R1 , I , J ) = C( I, J ) 

50 CONTINUE 

DO 60 KM = 1,6 
MEAN ( R1 , KM) = RM(KM) 

60 CONTINUE 

WRITE ( OUT, * ) ' Would you like to call another data set ? ' 

READ( IN, ' (A) * ) ANSWER 

IF (ANSWER .EQ. 'YES' .OR. ANSWER .EQ. »Y') GOTO 5 

IF (ANSWER .EQ. 'yes' .OR. ANSWER .EQ. ' y ' ) .GOTO 5 

... The subroutine CONECT (Gygax) takes as input the number 

of data sets Rl, and the left/right array ID matrix 
IND2 and determines whether the problem input by tii< 

user is connected. That is, all sensor arrays in tr. « 

problem communicate with one another via a string oi 
crossover data sets. If this test fails, then the 
output variable TESTC is zero and the user is prom pi 
to use one of the three options listed in the V/ RITE 
statements bel ow . 

... If the problem is connected, then TESTC is not zero art 
the subroutine returns a value for: 

K> the number of sensor arrays in the problem. 

IND1, a useful conversion of IND2. 

IA, the list of sensor ID's in the order 
maintained by the prog rai;;. 

IND2R, the submatrix of IND2 that represents dote, ss 
connected to the first data set. 

DATSET, the number of crossover data sets connect to 
to the first input data set. 

CALL CONECT ( OUT, Rl , IND2, K, IND1 , I A, TESTC, IND2R, DATSET) 

IF (TESTC .EQ. 0) THEM 

WRITE(OUT,*) 'All of your arrays are not connected. ' 
ViRITE(OUT,*) 'Do you want to :' 

WRITE(0UT>*) ' (1) Quit now' 

WRITE(0UT,*) ' (2) Input more data' 

WRITE(0UT»*) ' (3) Continue, using the first connected seb 

65 WRITE(0UT,*) 'Please enter the number of your choice ' 

RE AD ( I N , * ) I 

IF (.NOT. ((I .EQ. I). OR. (I .EQ. 2). OR. (I .EQ. 5))) GOTO 65 
GOTO (66, 67, 68), I 

6 6 WRITE (OUT,*) 'Program terminating. Adi os. Amigo !' 

STOP 



44 



oo ooooooo oooo ooooooooo ooooooooooooo ooooooo 



GOTO 5 



67 

... The subroutine REDUCE (Gygax) does nothing unless option 3 
above is evoked. In that case it modifies the 
variable CROSSA, MEAN, R1 , K, INDl, and IA so that 
they contain only the information required by the 
connected problem called for by this option. 

68 CALL REDUCE(CROSSA, MEAN, R1,K, INDl, IA,IND2R,DATSET) 

END IF 

... Feed the arrays output by CONECT or REDUCE into POOL: 

The subroutine POOL checks on the number of data sets 
associated with each pair of sensor arrays. If this 
value is one or zero it does nothing. Otherwise it 
(in effect) pools all the data associated with each 
unique sensor array pair into one data sex (using trie 
within groups sum of squares technique for crossprocucts, 
and the weighted average technique for means. Thus, 
CROSSA is converted to DEV; MEAN to XE; R1 to E; INDi to 
IND; and NS1 to NS. The first six called variables are 
input and the last five are output. 

CALL POOL ( K , R1 , INDI, CROSSA, ME AN, MSI, IND, DEV, XB,R, MS) 

... Array IND output by POOL goes into INVIND: 

The subroutine INVIND takes the variables K , R , anc IMD 
and outputs the K by K matrix FF. This is ar, u r p e r 
triangular incidence matrix which contains a one in 
the position i , j ( i < j ) only if there exists a set of 
crossover data involving both the i-th and j-ti 
sensors (as indexed in IA). FF is essentially an 
inverse of IND . 

CALL IKVIND(K, R, IND, FF) 

... Array FF is output from INVIND arid input to MULTAR. 

... Set epsilon, the tolerable error level used in the 
stopping rule of the subroutine MULTAR: 

EP = l.D-6 

... The subroutine MULTAR computes, cy iteration, the K 

orthonormal re-orientation matrices for e j ach sensor 
listed in IA. Since these are all relative to the 
first sensor, the first matrix in the output 3 is 
the 3 x 3 identity matrix. 

CALL MULTAR(EP,FF,DEV, K , R, B ) 

... Array B is output from MULTAR and input to INTCEP: 

CALL INTCEP(FF,NS,B,XB,K,P.,A) 



45 



ooo ooooooo oooooo oooo oooooooooo 



... Array A is output from INTCEP and input to DISPLC: 

... The subroutine DISPLC takes variables K , B , A , ana IA as 

input/ reads the sensor array location file and cornput 
the estimated displacement of each of the K sensors in 
the problem. The output DEL is the set of displacemen- 
vectors and D is the corresponding set of K displace me i 
distances. Since these are all relative to the first 
sensor» the first row of DEL and first element of D an 
zero . 

CALL DISPLC(K,B, A, IA,DEL,D) 

... Prepare the sequential computation of the maximum angles 
of sensor rotation. 

DO 75 IK= 1 , K 

DO 70 1=1,3 

DO 70 J = 1 , 3 

70 BB ( I , J ) = B ( I K , I,J) 

... Array BB is input to MAXANG 

... The subroutine MAXANG takes a 3 by 3 orthonormal matrix B5 
and computes PP, the largest possible rotation angle 
experienced by any vector transformed by BB. 

CALL MAXANG ( BB, PP) 

P(IK) = PP 
75 CONTINUE 

... The subroutine ANGLES converts each of the K c rthoncrmo 1 
matrices in B to Their representation as the three 
Euler angles: Roll, Pitch, and Yaw. Since these are 

relative to the first sensor, the first row of this 
matrix is zero. 

CALL ANGLES (K, 6, EA) 



... Prepare to write the output to files: 

OPE N ( 3, FI LE=' KEYFIL1.DAT* , STATU S= ' OLD ' ) 

0PEN( 4, FILE= 'KEYFIL2 . DAT ' , STATU S= 'OLD ' ) 

OPEN ( 7 , FILE= 'KEYFIL3 . DAT' , STATU S= ' OLD ' ) 

OPEN (8, FILE*' KEYFIL4.DAT' , STATU S= 'OLD' ) 

C 

WRITE ( OUT, * ) ' Displacement arid rotation (magnitude):' 

DO 77 IK = 1 , K 

C ... Write the vectors IA, D and P to FILE1 

W R I T E ( 3 , 8 5 ) IA(IK), D(IK), P(IK) 

WRITE (OUT, 85 ) I A ( IK) , D(IK), P(IK) 

C 

C ... Write the vector IA and matrices A and DEL to FILE2. 

WRITE(4,80) IA(IK), ( DEL ( I K , L ) , L= 1 , 3 ) , ( E A ( I K , LK ) , LK = 1 , 3 ) 
77 CONTINUE 
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WRITE(0UT,*) ' Displacement vectors and Euler angles 
DO 78 IK = 1 , K 

WRITE ( OUT , 80 ) IA(IK), ( DEL ( IK, L ) , L= 1 , 3 ) , ( EA( IK, LK ) , LK= 1 , 3 ) 

78 CONTINUE 

80 FORMAT (IX, 15 , IX, 3 F 1 2 . 4 , IX , 3 ( IX , F10 . 7 ) ) 

85 FORMAT (2X, 15 , 2X, F10 . 2, 2X, F10 . 6 ) 

C 

C ... Write the three dimensional array B to FILE3. 

WR ITE ( OUT , * ) ' * 

WRITE ( OUT , * ) ' Array B output by KEYMAIN: ' 

DO 90 IK = 1 , K 
WRITE ( OUT , * ) ' » 

WRITE (OUT, *) » * 

DO 90 I = 1,3 

WRITE (7, 100) (B(IK,I,J),J=1,3) 

WRITE (OUT, 100)(B(IK,I,J),J=1,3) 

90 CONTINUE 

100 FORMAT ( 2X, 3 F15 . 10 ) 

C 

DO 105 IK = 1 , K 

WRITE ( 8, 115 ) (A(IK,J),J=1,3) 

105 CONTINUE 

115 F0RMAT(2X,3F15.5) 

C 

WRITE ( OUT, 1 1 0 ) 

110 FORMAT(//,' Program terminating. Operation complete. Eye. ') 

C 

CL0SE(UNIT=3 ) 

CLOSE ( UN IT=4 ) 

CLOSE ( UNIT-7 ) 

CLOSE (UNIT-8) 

C 

C 

STOP 

END 



SUBROUTINE CROSSP ( M, AA, MEAN, CROSSA) 

****** ******************************************* ********** ******* 
*** THIS PROGRAM CALCULATES THE CROSSPRODUCT DEVIATIONS FC\ ** 

*** THE RAW DATA ARRAY AA (A MATRIX FOR EACH CROSSOVER SET) ** 

*** THE SET OF MEANS IS DEVELOPED AND LABELED "MEAN" 

*** CROSSA IS THE SUM OF CROSSPRODUCT DEVIATIONS FROM THE MEAN 
****************************************************************** 

INTEGERS N 

REAL*6 AA ( 200 , 6 ) , MEAN ( 6 ) , CR0SSA(3,3), SUM 

RE AL * 8 SUMXX , SUMXY, SUMXZ, SUMYX, SUMY Y , SUMYZ, SUMZX, S'JMZY 
REALMS SUMZZ , XX, XY, XZ, YX, YY, YZ, ZX, ZY, ZZ 

... N is the number of rows in the data matrix A A 
... Compute the vector of means. 
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do 20 J = 1,6 
SUM = 0 . DO 
DO 10 I = 1,N 
SUM = SUM + AA( I, J ) 

10 CONTINUE 

MEAN ( J ) = SUM / DBLE(N) 
20 CONTINUE 



... Begin the crossproduct operations: 

Initialize the nine sums to zero. 

SUMXX=0 . DO 
SUMXY = 0 . D 0 
SUMXZ=0 . DO 
SUMYX=0 . DO 
SUMYY=0 . DO 
SUMYZ=0 . DO 
SUMZX-O.DO 
SUMZY=0 . DO 
SUMZZ = 0 .DO 

DO 100 IA = 1 » M 

... Create the crossproduct deviations for row IA of matrix AA 

XX= ( AA ( I A, 1 ) -MEAN ( 1) ) * ( AA ( I A , 4 ) -ME AN ( 4 ) ) 

XY=(AA(IA,1)-MEAN(1) ) * ( AA ( I A, 5 ) -MEAN ( 5 ) ) 

XZ=(AA(IA,1)-MEAN(1) ) * ( AA ( I A, 6 ) -MEAN ( 6 ) ) 

YX=(AA( IA,2)-MEAN(2) )*(AA( IA, 4 ) -MEAN ( 4 ) ) 

YY=(AA(IA,2)-MEAN(2) ) * ( AA ( IA, 5 ) -MEAN ( 5 ) ) 

YZ=(AA( IA,2)-MEAN(2) )*(AA( I A, 6 ) -ME AN ( 6 ) ) 

ZX=(AA(IA,3)-MEAN(3) )*(AA(IA>4)-MEAN(4) ) 

ZY=(AA( IA,3)-MEAN(3) ) * ( AA ( I A, 5 ) -ME AN ( 5 ) ) 

ZZ=(AA(IA,3)-MEAN(3) ) * ( AA ( I A, 6 ) -MEAN ( 6 ) ) 

C 

C ... Accumulate the sums: 

SUMXX = XX + S UMXX 
SUMXY = XY + SUMXY 
SUMXZ-XZ+SUMXZ 
SUMYX=YX+SUMYX 
SUMYY= Y Y+SUMY Y 
SUMYZ=YZ+SUMYZ 
SUMZX-ZX+SUMZX* 

SUMZY=ZY+SUMZY 
S U M Z Z = Z Z + S U M Z Z 
C 

100 CONTINUE 
C 
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C ... Create the CROSSA matrix: 

CR0SSA(1,1)=SUMXX 
CR0SSA(1*2)=SUMXY 
CR0SSA( 1*3 )=SUMXZ 
CR0SSA(2»1)=SUMYX 
CR0SSA(2*2)=SUMYY 
CR0SSA(2»3)=SUMYZ 
CR0SSA(3*1)=SUMZX 
CR0SSA(3*2)=SUMZY 
CROSSA(3»3)=SUMZZ 
C 

RETURN 



C 



END 



SUBROUTINE CONECT (OUT, R1 , IND2, K, IND1 , 1 A, TESTC, IND2R, DATSET) 

a#####**###*###*##*###*#**####*#**#*#*#****###**************#*** 
*** This subroutine checks for the connectedness of the *** 

*** input data sets. If the problem is connected then the *** 

*** user is informed and the array pairs are printed on the *** 

*** screen; if not connected* then the user is prompted to *** 

*** select one of three options - quit* aod conecting data * * * 

*** sets* or run the program using the first connected set *** 
*** that was input. Gygax-Julyl985 * * * 

a******************************************************** ¥■ ****** 

...Variable declarations. 

INTEGERS R1*K* IND2( 2*30), IND1 (30,3 0)* I* J, I A ( 3 0 ) * FIRST 
INTEGERM LIST( 30 ), BEGIN* HALT* D ISCON, L, M, 0, TESTC* IND2R(2,30) 

I NTEGER*4 D ATSET ( 3 0 ) , COUNT , S AVE ( 2 * 3 0 ) , OUT 

...Initialize the values of FIRST and COUNT: 

FIRST = 0 
COUNT = 0 

...Make vector IA = list of all arrays (w/o repeats) in I ND2 
and get the value for K = § of individual arrays. 



I A ( 1 ) 


= 


I ND2 ( 1 * 


1 ) 


I A ( 2 ) 


= 


I N D 2 ( 2 * 


1) 


K = 


3 








IF 


(P.l 


• 


EQ. 1) 


GOTO 


DO 


50 


I 


= 1 , R1 






DO 


40 


J = 1, 


2 






M 


= K - 1 








DO 


30 L = 


1*M 



IF ( I MD2 ( J , I ) .EQ. I A ( L ) ) GOTO 40 
30 CONTINUE 
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40 

50 

60 

c 



70 

80 



131 



140 



141 

150 



151 

160 

170 



IA(K) = IND2(J,I) 

K = K + 1 
CONTINUE 
CONTINUE 
K = K - 1 

WRITE ( OUT, * ) ' R1 ' , R1 
WRITE ( OUT , * ) 1 K »,K 

... For each column of IND1 (columns correspond to data sets) t\ 
entries are all zero except for the row that corresponds to 
the left array (= 1) and the right array (= 2). 



DO 80 I = 1 , R1 
DO 70 J = 1 , K 
INDKJ, I) = 0 

IF ( I ND2 ( 1 , I ) .EQ. I A ( J ) ) INDKJ, I) = 1 
IF ( I ND2 (2,1) .EQ. I A ( J ) ) INDKJ, I) = 2 
CONTINUE 
CONTINUE 



Check to see 



if all the arrays are connected. 



TESTC = 1 
LIST(l) = - I A ( 1 ) 

DO 131 I = 1 , R1 

IF ( I ND2 ( 1 , I ) .EQ. -LIST(D) I ND2 ( 1 , I ) = - I ND 2 ( 1 , I ) 

IF ( I ND2 ( 2, I ) .EQ. -LIST(l)) IND2(2,I) = -IND2(2,I) 

CONTINUE 
BEGIN = 1 
HALT = 1 

IF (.NOT. (BEGIN .LE. HALT)) GOTO 170 
NODE = L I ST ( BEG I N ) 

BEGIN = BEGIN + 1 
DO 150 I = 1,R1 

IF ( .NOT. ( (NODE . EQ. IND2( 1, I) ) . AND. ( IND2(2, I) .GT. 0) ) ) GOTO 150 
HALT = HALT + 1 
L IST( HALT) = - IND2 (2,1) 

DO 141 J = 1 , R1 

IF ( IND2 ( 1 , J ) .EQ. -L I ST( HALT) ) I N D 2 ( 1 , J ) = -IND2(1,J) 

IF ( I ND2 ( 2 , J ) .EQ. -LI ST (HALT) ) IND2(2,J) = -IND2(2,J) 

CONTINUE 
CONTINUE 
DC 160 I = 1,R1 

IF ( .NOT. ( (NODE. EQ. IND2(2, I) ) . AND. ( IND2( 1, I) .GT. 0) ) ) GOTC 160 
HALT = HALT + 1 
LIST (HALT) = - I N D 2 ( 1 , I ) 

DO 151 J = 1 , R1 

IF ( I ND2 ( 1 > J ) .EQ. -L 1ST ( HALT) ) I ND 2 ( 1 , J ) = -IND2(1,J) 

IF ( I ND2 ( 2 , J ) .EQ. -LIST(HALT) ) IND2(2,J) = -IND2(2,J) 

CONTINUE 
CONTINUE 
GOTO 140 
CONTINUE 
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C ...Print out the matched pairs. 

DISCON = 0 
WRITE ( OUT >230) 

DO 200 I = 1,R1 

IF ( IND2 ( 1 * I ) .LT. 0) GOTO 190 
IF ( I ND2 ( 1 , I ) .EQ. 0) GOTO 200 

IF ( ( IND2 (1,1) .GT. 0) .AND. (DISCON .EQ. D) GOTO 200 

FIRST = FIRST + 1 

DISCON = 1 

TESTC = 0 

BEGIN = 1 

HALT = 1 

LIST(l) = IND2 (1,1) 

GOTO 200 

190 WRITE ( OUT, 240 ) - I ND2 ( 1 , I ) , - I ND2 ( 2 , I ) 

IF ( (FIRST. EQ.O) .OR. ( (FIRST. EQ.l) .AND. (DISCON. EQ.l) ) ) THEN 
COUNT = COUNT + 1 
IND2R(1, COUNT) = -IND2(1,I) 

IND2R(2, COUNT) = -IND2(2,I) 

DATSET ( COUNT) = I 
END IF 

SAVE(1,I) = - IND2 ( 1 , I ) 

SAVE (2,1) = -IND2(2,I) 

IND2 ( 1 , I ) = 0 
I ND2 ( 2 > I ) = 0 
200 CONTINUE 

IF (DISCON .EQ. 1) GOTO 140 
DO 220 I = 1,R1 

IND2(1,I) = S AVE ( 1 > I ) 

IND2(2,I) = SAVE ( 2, I ) 

220 CONTINUE 
RETURN 

230 FORMAT ( IX, * THE FOLLOWING PAIRS ARE CONNECTED :») 

240 FORMAT ( IX > 14 15 ) 

END 



SUBROUTINE REDUCE ( CROSSA, MEAN , P.l , K, IND1 , I A, IND2R, DATSET) 

***************** **********#**********************k***x* x * * * * * 

* * * This is a specialized subroutine that is used when * * * 

* * * option three is involked as a result of a failed con- * * * 

* * * nectedness test. The disconnected data sets must be * * * 

*** removed from the variables CROSSA and MEAN* and ether *** 
*** program supporting variables must be adjusted. * * * 

*** Gygax - July 1985 *** 

a##**##**#*###############*#*#####****#*#*####**** ****** 

... Variable declarations. 

INTEGERS R1,K, IND1(30,30),IA(30), IND2R(2,3C) , I , J , L , M , DATS ET ( 3 0 ) 
C 

REAL *8 CROSS A (30, 3 ,3 ) , ME AN (3 0,6) 

C 
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C ... Compute the new* reduced R1 : 

DO 10 I * 1,30 

IF ( IND2R( 1,1) ,EQ. 0) GOTO 20 
10 CONTINUE 
20 R1 = I - 1 

... Make a new, reduced vector IA = list of all arrays in 
IND2R w/o repeats. Also, compute a new K. 

I A ( 1 ) = IND2R( 1 , 1 ) 

I A ( 2 ) = I ND2R ( 2 , 1 ) 

K = 3 

IF ( R1 .EQ. 1) GOTO 60 
DO 50 I = 1 , R1 
DO 40 J = 1,2 
M = K - 1 
DO 30 L = 1 , M 

IF ( I ND2R ( J , I ) .EQ. I A ( L ) ) GOTO 40 
30 CONTINUE 

I A ( K ) = IND2RC J , I ) 

K = K + 1 
40 CONTINUE 

50 CONTINUE 
60 K = K - 1 

... Remake the reduced matrix IND1 - for each column in IND1 
(corresponding to a data set) the entries are zero except 
for the enties corresponding to the left array (= 1) and 
right array (= 2). 

DO 80 I = 1 , R1 
DO 70 J = 1 , K 
I ND 1 ( J , I ) = 0 

IF ( I ND2R ( 1 , I ) .EQ. IA(J)) IND1(J,I) = 1 
IF ( I N D 2 R ( 2 , I ) .EQ. I A ( J ) ) I ND 1 ( J , I ) = 2 
70 CONTINUE 

80 CONTINUE 

... Reduce the arrays CROSSA and MEAN to account for the 
removed data sets. 

DO 120 I = 1,R1 
DO 90 J = 1,6 

MEAN ( I , J ) = ME AN ( DATS ET ( I ) , J ) 

90 CONTINUE 

DO 110 J = 1,3 
DO 100 L = 1,3 

CR0SSA( I, J ,L) = CROSSA(DATSETd), J,L) 

100 CONTINUE 

110 CONTINUE 

120 CONTINUE 
RETURN 
END 
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SUBROUTINE P00L(K,R1, IND I , CROS S A , ME AN , NS 1 , IND , DE V , XB , R, NS) 



a******###******####**#*###****#####****####****######********* 

*** Pools the means and the cross product deviations *** 

*** so that each cross covariance corresponds to a unique *** 

*** sensor array pair. * * * 

*##*^######*####*#####*#**##**#**###* a****************'********* 



INTEGERS K,R1,IND1(30,30),IND(30,30),R,NS1(30),NS(30),IC,IS 
REAL*8 CROSS A( 30,3,3), MEAN(30,6), DEV(30,3,3), XB(30,6), 

+ TX(6)» DD(3,3),RNS, DNS1 



... Outputs replacement variables IND, DEV, XB, R» and NS 
for use in MULTAR and INTCEP. That is: 

IND replaces IND1, DEV is the pooled version of CROSSA» 

XD is the pooled version of MEAN* R replaces Rl, 
and NS repl aces NS1 . 

... Identify data subsets in IND1 which are in the wrong orcer 
for use in the algorithms and transpose them. 

DO 20 I R= 1 > R1 
DO 10 1 = 1, K 

IF ( IND1 ( I , IR) .EQ. 0) THEN 
GOTO 5 

ELSE IF ( I ND 1 ( I , I R ) .EQ. 1) THEN 
IK= I 

ELSE IF ( IND1 ( I , IR) .EQ. 2) THEN 
J K= I 

END IF 

5 CONTINUE 
10 CONTINUE 

IF (IK .LT. JK) GOTO 20 
DO 12 1=1,3 
IP3= 1+3 

TX ( I ) =MEAN ( I R, IP3 ) 

TX( IP3 ) =MEAN( IR, I ) 

DO 12 J = 1 , 3 

12 DD ( I , J ) = CROSSA( IR, J , I ) 

DO 15 1=1,3 
IP3=I+3 

MEAN(IR, I ) =TX ( I ) 

ME AN ( IR, IP3 ) =TX( IP3 ) 

DO 15 J = 1 , 3 

15 CROSSA(IR,I,J)=DD(I,J) 

20 CONTINUE 

... The transpositions are completed. 

... Locate the subsets to be pooled and do the pooling. 

Initialize the variables. 

IC = 1 

C ... Zero the IND array. 

DO 25 I T = 1 , K 
DO 25 I R= 1 , R1 
IND ( IT, IR) =0 
25 CONTINUE 
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C ... Start the algorithm. 

KM1-K-1 
DO 40 1=1, KM1 
IP1 =1+1 
DO 40 J= IP1 , K 
IS = 0 

C ...Zero the arrays initially. 

NS(IC) = 0 
DO 28 IT= 1,3 
ITP3 = IT+3 
TX ( IT) =0 . 0D+0 
TX ( ITP3 ) =0 . 0D+0 
DO 28 J T= 1 , 3 

28 DEVCIC, IT,JT)=0.0D+0 

... Initialization is completed for this I,J pair. 

DO 35 I R= 1 , R1 

IF ( IND1 ( I , IR) .EQ. 0 .OR. IND1(J,IR) .EQ. 0) GOTO 35 
DN SI = DBLE(NSKIR) ) 

C ... Set a flag to show a viable I , J pair: 

IS=1 

C ... Accumulate weighted sum of means: 

DO 32 IT= 1 , 6 

TX ( IT) = TX ( IT ) + DNS 1 * MEAN ( IR, IT ) 

32 CONTINUE 

C ... Accumulate sums of c ros s p rod uct s . 

DO 33 IT= 1 , 3 
DO 33 J T= 1 , 3 

DEVCIC, IT, JT) = DEVCIC, IT, JT) + CR0S SA ( I R , IT , J T ) 



33 


CONTINUE 




C 


... Accumulate 


samp 1 e sizes 




NSC IC ) = NSC IC ) 


+ NS1CIR) 


35 


CONTINUE 






IF (IS .EQ. 0) 


GOTO 40 



C ... Convert pooled crossproducts into cross covariances. 

RNS = DBLECNSCIC)) 

DO 36 IT-1,3 
DO 36 J T= 1 , 3 

36 DEVCIC, IT, JT)=DEV(IC, IT, JT) /RNS 
C ... Convert weighted sums into pooled means: 

DO 38 IT= 1 , 6 

38 XB ( IC, IT) = TXCIT) / RNS 

C ... Set the values in the IND matrix and update the counter 

I ND ( I , I C ) = 1 
INDCJ , IC) = 2 
IC = IC + 1 
40 CONTINUE 

C ... When finished, the counter needs correction. 

R= IC - 1 

RETURN 

END 
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SUBROUTINE I NV I ND ( K , R, I ND , FF ) 



a**************************************************************** 



*** Computes the matrix FF from IND * * * 
*** FF is used in MULTAR and INTCEP * * * 
* * * IR indexes the columns of I N D > i.e. the crossover pairs. * * * 
*** I and J index the sensor arrays. *** 
*** K is the number of arrays in the problem *** 



*** R is the number of (array pair) data sets in the problem. *** 
a#*#***************#******#**####**#*#*#****#**#**#************** 



I NTEGER*4 R, K 

INTEGERS IND(30,30), FF(30,30) 

... Initialize F : 

DO 20 1=1, K 
DO 20 J = 1 , K 

... Change the value of FF to the crossover set index for 
those array pairs, I < J , that are in the problem. 

20 FF ( I , J ) = 0 

KM1 = K - 1 
DO 40 1=1, KM1 
IP1 =1+1 
DO 40 J = IP1 , K 
DO 40 IR = 1 , R 

I F ( I ND ( I , I R) .EQ. 1 .AND. I ND ( J , I R ) .EQ. 2) FF(I,J) = IR 
40 CONTINUE 

RETURN 
END 



SUBROUTINE MULTAR ( EP, FF , DEV , K, R, B ) 

#**************#****#**#************************#*******#**xx**** 
*** This is an iterative program to compute the K o rt ho n o rma 1 *** 
*** matrices in B. They must simultaneously opt in. ize the sum *** 
*** of traces objective function. * * * 
***************************************************************** 



** K is the number of arrays. 

* * R is the number of crossover sets. 

* * EP is the epsilon for stopping the iterations. 

* * DEV is the set of pooled crosscovariances. 

** Calls BMAX , MOE and MULT3 . 



. . . Declarations 
INTEGERM R, FF(30,30), K 

RE AL * 8 B ( 3 0 , 3 , 3 ) , E(30,3,3), F(30,3,3), DEV(30,3,3) 

RE AL *8 EP, REP, W ,W0, BB(3,3), DD<3,3), T(3,3), 0(30, 3, 3) 
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... Initialize B as K identity matrices. 

DO 2 KK= 1 , K 
DO 2 I = 1/3 
DO 2 J = l,3 
BCKK, I, J ) = 0 . DO 
IF (I .EQ. J) BCKK, I, J) = 1.D0 
2 CONTINUE 

... Initialize the temporary arrays. 

5 DO 10 KK=1,K 

DO 10 I =1/3 
DO 10 J = 1/3 
F ( KK, I , J ) =0 . DO 
E(KK, I, J )=0.D0 
0 CONTINUE 

... Compute WO/ the initial value of the objective function. 
CALL MOE(K,FF,DEV,B,WO) 

... Prepare for the linear transformation of those members of E 
designated as LEFT with the cross covariances. 

DO 30 KK=2 , K 
KKM1=KK-1 
DO 30 JI-l/KKMl 
I R=FF ( J I , KK ) 

IF (IR .EQ. 0) GOTO 25 

DO 15 IB = 1,3 

DO 15 JB = 1,3 

BBC IB, JB) = B( J I, IB, JB) 

DDCIB/JB) = DEV(IR,IB,JB) 

15 CONTINUE 

... Perform matrix multiplication and accumulate. 

CALL MULT3 (BB,DD,T) 

DO 20 I = 1,3 
DO 20 J = 1,3 

20 E ( KK , I , J ) = E ( KK , I , J ) + T(I,J) 

25 CONTINUE 

30 CONTINUE 

... Prepare for the linear t r an s f o rma t i c n s cf those members of t 
designated as RIGHT with the cross covariances. 

KM1 = K-l 
DO 40 K K = 1 , K M 1 
KKP 1 = KK + 1 
DO 40 JJ=KKP1,K 
I R= FF ( KK , J J ) 

IF (IR .EQ. 0) GOTO 38 

DO 32 IB = 1,3 

DO 32 JB = 1,3 

BBC IB, JB) = B( J J , IB, JB) 

DDC IB, JB) = DEVC IP, JB, IB) 

32 CONTINUE 

C 
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C ... Perform matrix multiplication and accumulate. 

CALL MULT3 (BB,DD,T) 

DO 35 I =1,3 
DO 35 J = 1,3 

35 F(KK,I,J)=F(KK,I,J)+T(I,J) 

38 CONTINUE 

40 CONTINUE 

C 

C ... Gather the LEFT and RIGHT accumulations. 

DO 50 KK=1,K 
DO 50 I =1,3 
DO 50 J = 1,3 

G(KK, I, J )=E(KK, I, J )+F(KK, I, J ) 

50 CONTINUE 

... Start seeking the maximum on the objective surface. 

DO 60 KK=2,K 
DO 55 I = 1,3 
DO 55 J =1,3 
BB( I, J ) = B(KK, I, J ) 

DD( I, J ) = G(KK, I, J ) 

55 CONTINUE 

... Subroutine call to optimize an indiviual matrix in B. 
CALL BMAX ( EP, DD , BB ) 

... Insert updated values into the B array. 

DO 58 1=1,3 
DO 58 J = 1 , 3 

58 B(KK, I, J ) = BB( I, J ) 

60 CONTINUE 



... Compute new value of the objective function. 

CALL M0E(K,FF,DEV,B,W) 

... Compare with previous value and decide whether to 
terminate or iterate. 

REP = EP * W0 / 1.D2 

IF (DABSCW - W0) .GT. REP) GOTO 5 

RETURN 

END 



SUBROUTINE MOE (K, FF, DEV , B , W ) 

*************************************************************** 
*** Computes the measure of effectiveness for MULTAR: *** 

* * * Output is W, the sum of traces of cross covariances * * * 

*** modified on the left by the B matrix of the left array, *** 

* * * and on the right by the transpose of the B matrix of *** 

*** the right array. *** 

*************************************************************** 

INTEGERS K, FF(30,30) 

REAL *8 B ( 30 , 3 , 3 ) , DEV(30,3,3), W 
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W=0 . DO 
KM1=K-1 

DO 20 IT = 1 , KMl 
ITP1= IT+1 
DO 20 JT = ITP1 » K 
IR = FF ( IT/ JT) 

IF (IR . E'Q. 0) GOTO 15 
DO 10 1=1/3 
DO 10 M= 1 / 3 
DO 10 J-1,3 

W=W+DEV(IR,I,M)*B(JT,J,M)*B(IT,J,I) 
10 CONTINUE 

15 CONTINUE 

20 CONTINUE 

C 

RETURN 

END 



SUBROUTINE BMAX (EP,D,B) 

******************************************************* ******* 



*** Iterative program that climbs the surface D*B-TRANS. *** 
*** and stops when the change is less than EP times *** 
*** previous level divided by 10. Input D from MU L TAR and * * * 
*** output B which is a 3x3 othonormal matrix. *** 
*** Calls the subroutine TWODIM. *** 
***__Augustl985. * * * 



************************************************************** 

RE AL*8 B ( 3 / 3 ) / D(3,3), BB(3,3), C(3,3), BM(3,3) 

REAL*8 Q, Q0, EP, R, REP 

... Initialize the value on the surface Q for the special 
case of B = Identity matrix: 

Q = 0 . DO 
DO 2 1=1,3 
Q = Q + D( I, I) 

2 CONTINUE 

... Initialize B as the identity matrix: 

DO 5 1=1,3 
DO 5 J = 1 , 3 
B( I, J ) = 0 . DO 

IF (I .EQ. J ) B( I, J ) = 1 .DO 

5 CONTINUE 

6 CONTINUE 

... Compute the intermediate values C = D * B - TRANS : 

DO 25 I = 1,3 
DO 25 J = 1,3 
C( I, J ) = 0 . DO 
DO 25 L = 1,3 

C( I , J ) = C( I , J ) + D ( I , L ) * B ( J , L ) 

25 CONTINUE 
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... Subroutine call to climb higher on the surface of Q: 

CALL TWOD IM ( C > BB ) 

DO 8 I = 1,3 
DO 8 J = 1,3 
BM( I, J ) = B( I, J ) 

8 CONTINUE 

... Replace B with the matrix product. This updates the B matrix. 
CALL MULT3 (BB,BM,B) 

... Record previous level on the surface: 

QO = Q 

... Compute new value on the surface: 

Q = 0 . DO 
DO 10 I = 1,3 
DO 10 J = 1,3 

10 Q = Q + (D( I, J ) * B( I, J ) ) 

... Prepare the stopping rule: 

R = Q - QO 

REP = EP * QO / 10. 

... Compares the relative gain with EP; stops if too small: 

IF (R.GT.REP) GOTO 6 



RETURN 

END 



SUBROUTINE TWODIM (D,B) 

*********************************************************** 
*** FORTRAN subroutine to compute new rol 1 > pitch* and *** 
*** yaw matrices (B1,B2,B3) and combine into a single * * * 

*** orthonormal transformation. Receives D from BN! AX, *** 
*** outputs B, calls MULT3 . *** 

#** __ June 1985. *** 

*************************************************** ******** 



... Dimension variables and declare them to be double precision: 
REAL*8 B(3,3), D(3,3), T(3,3), E(3,3) 

RE AL *8 F ( 3 , 3 ) , Bl(3,3), B2(3,3), B3(3,3) 

REAL*8 S, C, DEN0M, SS 

... Initialize the three matrices. 

DO 5 1=1,3 
DO 5 J = 1 , 3 
B 1 ( I , J ) =0 . DO 
B2 ( I , J ) =0 . DO 
B3( I, J ) = 0 . DO 
5 CONTINUE 



59 



oo ooooo o oooooo 



C ... Develop B1 (roll matrix) from input D: 

S = D ( 3 » 2 ) - D ( 2 > 3 ) 

C = D ( 2 , 2 ) + D ( 3 > 3 ) 

SS = S*S + C*C 
DENOM = DSQRT(SS) 

S = S / DENOM 
C = C / DENOM 
C 

C ... Finalize the B1 matrix: 

B1 ( 2 » 2 ) = C 
B1 ( 2* 3 ) = S 
B1 (3 , 2 ) = -S 
B1 ( 3 > 3 ) = C 
Bl(l.l) = 1 . DO 

... Update the input matrix to adjust for roll: 

. . . E = D*B1 
CALL MULT3(D,B1,E) 

... Develop B2 (pitch matrix) from E: 

S = E ( 3 > 1 ) - E(l, 3) 

C = E ( 3 , 3 ) + E ( 1 , 1 ) 

SS = S*S + C*C 
DENOM = DSQRT(SS) 

S = S / DENOM 
C = C / DENOM 

... Finalize the B 2 matrix: 

B2 ( 1 » 1 ) = C 
B2 ( 1 , 3 ) = S 
B2 ( 2 , 2 ) = 1 . DO 
B2 ( 3 » 1 ) = -S 
B2 ( 3 > 3 ) = C 

... Additional update of the input matrix to adjust for pitch 
. . . F = E*B2 
CALL MULT3 (E,B2, F) 

... Develop B3 (yaw matrix) from F: 

S = F ( 2 , 1 ) - F ( 1 , 2 ) 

C = F ( 1 , 1 ) + F ( 2 , 2 ) 

SS = S*S + C*C 
DENOM = DSQRT(SS) 

S = S / DENOM 
C = C / DENOM 

... Finalize the B3 matrix: 

B3 ( 1 , 1 ) = C 
B3 (1,2) = S 
B3 ( 2 , 1 ) = -S 
B3 ( 2 , 2 ) = C 
B3 ( 3 , 3 ) = 1 . DO 
C 
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... Final triple multiplication of matrices coupled 
with transposition: 

B_transpose = B1 * B2 * B3 
DO 30 1=1,3 
DO 30 J = 1 , 3 
B ( J , I ) = 0 . DO 
DO 30 I K= 1 » 3 
DO 30 K J = 1 , 3 

B(J,I)=B(J,I)+B1(I,IK)*B2(IK,KJ)*B3(KJ,J) 

30 CONTINUE 

RETURN 
END 



SUBROUTINE MULT3(A,B,C) 

a####*#*##*##**#####****###*#*******##** ******************* 

* * * Multiplies the 3 by 3 matrices A and B to get C *** 
******************************************************** *** 



REAL*8 A ( 3 , 3 ) , B(3,3) , C(3,3) 

C 

DO 10 1=1,3 
DO 10 J=1 ,3 
C( I, J )= 0 . DO 
DO 10 K=1 ,3 

C C I , J ) = C(I,J) + A(I,K)*B(K, J) 
10 CONTINUE 
RETURN 
END 



SUBROUTINE I NTCEP ( FF , NS , B , XB , K , R , A ) 

C 

q *********************************************************** ****•« 

C *** Sets up and solves the system of linear equations for *** 

C *** each component of the set of K intercept vectors. *** 

C *** Requires the output of POOL and MULTAR. * * * 

C *** -- June 1985. *** 

Q ***************************************************** -XXX******** 

C 

C * * K is the number of sensor arrays in the problem. 

C ** R is the (pooled) number of crossover data sets. 

C ** NS is the vector of sample sizes for the crossover sets. 

C ** FF is the upper triangular K by K matrix whose values are 

C * * either zero or the index of the data set for the array pair 

C ** identified by the subscripts. 

C * * XB is the R by 6 matrix of means for each of the two arrays 

C ** identified with each of the R data sets. 

C ** B is the K by 3 by 3 collection of orthonormal orientation 

C * * correction matrices returned from MULTAR. 

C ** The output A is the K by 3 matrix of intercept values 

C * * estimated for each of the K arrays in the problem. 

C 
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... Calls MMATMUL which performs linear transformations. 

... Calls SYSLIN which solves systems of linear equations. 

... Declarations. 

INTEGERS R, FF(30,30), NS(30), K, I, IR, IP1 
REAL*8 BOO, 3, 3), XB (30, 6), MOO, 30), 

1 LHS(30,3),XL(30,3),XLL(30,3),XXL(30,3) , XR ( 3 0 , 3 ) , XRR ( 3 0 , 3 ) , 

2 XXR (3 0 > 3 ) > E ( 3 0 , 3 ) , F ( 3 0 , 3 ) , A ( 3 0 , 3 ) ,AA(30,31) ,DNS 

... Begin development of the coefficient matrix M » off diagonal 
terms. Also prepare the inputs for the summands on the 
other side of the equation. Affixes L and R are used to 
suggest the left and right portions of the expressions. 

... The values in the coefficient matrix are zero whenever the 
array pair is not identified with a crossover data set, i.e 
when F F ( i > j ) = 0 and i is less than j. Otherwise its value is 
the negative of the sample size for the data set. 

K M 1 = K - 1 
DO 10 1=1, KM1 
IP1 =1+1 

DO 10 J = I PI , K , 

IR=FF( I, J ) 

IF (IR .EQ. 0) THEN 
M ( I > J ) = 0 . DO 
M ( J » I ) = 0 . DO 
GOTO 10 

END IF 

M ( I , J ) = - 1 . DO * DBLE ( NS ( IR) ) 

M ( J , I ) = M ( I , J ) 

DO 5 11=1,3 

...The weighted version requires that the means be multi plied 
by NS. The first three columns of XB are identified with the 
left (L) array of the pair; the last 3 witti the right (P. ). 
This prepares inputs for summands on the other side of the 
equation. 

DNS = DBLE ( NS ( I R) ) 

XL ( IR, II ) =DNS*XB( IR, 1 1 ) 

IIP3 =3+11 

5 XR(IR,II)=DNS*XB(IR,IIP3) 

10 CONTINUE 

... Finish development of the M matrix. The diagonal term in a 
row is the negative of the total of the other terms in that 
row. The first column of E is used to accumulate terms. 



62 



o Oo o ooo ooooo 



12 



DO 15 1 = 1, K 
M ( I , I ) = 0 . DO 
E ( I , 1 ) = 0 . DO 
DO 12 J = 1 > K 

E ( 1 , 1 ) = E ( 1 , 1 ) + M ( I > J ) 

M ( I , I ) = - 1 . DO * E ( I , 1 ) 

15 CONTINUE 

... The coefficient matrix is complete. 

Next develop the sequence of partial sum arrays for use in 
the LHS of the system of equations. First initialize. 

Then treat the left array partial sums. 

DO 25 KK=2,K 
DO 22 J=1 ,3 
XXL ( KK , J ) =0 . DO 
XLL(KK, J )=0.D0 

22 CONTINUE 
KKM1=KK- 1 

DO 25 1=1, KKM1 
IR = FF ( I , KK ) 

IF (IR .EQ. 0) GOTO 25 

CALL MMATML (K,R,I,IR,IR,B,XL,F) 

DO 23 J=1 , 3 

XXL ( KK , J ) = XXL ( KK , J ) + F(IR,J) 

23 XLL ( KK , J ) = XLL ( KK , J ) + XR(IR,J) 

25 CONTINUE 

... Next develop the right array partial sums. 

Again the initial values must be zero. 

DO 30 KK= 1 , KM1 
DO 26 1=1,3 
XXR ( KK , I ) =0 . DO 
XRR(KK, I )=0.D0 

26 CONTINUE 
KKP1=KK+1 

DO 30 J = KKP 1 , K 
I R= FF ( KK , J ) 

IF (IR .EQ. 0) GOTO 30 

CALL MMATML ( K , R, J,IR,IR,B,XR,F) 

DO 27 1=1,3 

XXR ( KK , I ) = XXR ( KK , I ) + F ( I R , I ) 

27 XRR ( KK , I ) = XRP. ( KK , I ) + XL(IR,I) 

30 CONTINUE 

...Collect the above quantities into the LHS of the system 
of equations. Omit the end terms for now. 

DO 35 KK= 2 , KM1 
DO 31 11=1,3 

31 XL ( K , I I ) = XL L ( KK , 1 1 ) + XRR(KK,II) 

CALL MMATML (K,R,KK»K,K,B,XL,F) 

DO 33 11=1,3 

3 3 LHS(KK, II)=XXL(KK> II)+XXR(KK, 1 1 ) — F (K, II) 

35 CONTINUE 

C 
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...Finalize with the end correction terms. 

CALL MMATML(K,R,1,1,1,B,XRR,F) 

DO 38 I = 1,3 

LHS(1,I) = XXR(1,I) - F(1,I) 

38 CONTINUE 

CALL MMATML(K,R,K,K,1,B,XLL,F) 

DO 40 1=1,3 

40 LHS ( K , I ) = XXL ( K , I ) - F(1,I) 

... SOLVE THE LINEAR SYSTEM MA= L . This must be done three times 
once for each column of LHS. Since the matrix M has rank K- 
and since the solution is to be made relative to the first 
array, we trim away the first row and first column of M to 
get a non-singular system. For the same reason the elements 
of the first row of A are all set to zero. 

DO 50 1=1,3 

... Prepare the input to SYSLIN for each column of LHS. 

DO 48 KI=2, K 
DO 45 KJ = 2, K 
KIM1=KI-1 
KJM1=KJ-1 

AA(KIM1,KJM1)=M(KI,KJ) 

45 CONTINUE 

48 AA ( K IM1 , K ) = LHS ( K I , I) 

... Solve the system. 

CALL SYSLIN(AA,30,KM1 ) 

A( 1, I) = 0 . DO 

... Place the solution into the output matrix A. 

DO 50 KK = 2 > K 
KKM1=KK-1 

A(KK, I)=AA(KKM1,K) 

50 CONTINUE 



RETURN 

END 



SUBROUTINE MULM AT ( BB , EE , EF ) 

XX**#******#****###***#**XXX#*****#X***##*#*X********XXX*X 

*** Subroutine to perform a linear transformation in *** 

*** three-space. Specifically, EF is the product of * * * 

* * * the matrix BB and the vector EE. * * * 

RE AL *8 BB ( 3 , 3 ) , E F ( 3 ) , EE ( 3 ) 

... Initialize EE at zero: 

DO 10 I = 1,3 
EF ( I ) = 0 . DO 
10 CONTINUE 
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DO 20 I = 1,3 
X = 0 . DO 

C ... Accumulate the dot product of EE and the I-th row of BB: 

DO 30 J = 1,3 

30 X = X + BB( I, J ) * EE( J ) 

EF ( I ) = X 
20 CONTINUE 
RETURN 
END 



SUBROUTINE SYSL IN ( A, IR, IC) 

*************************************************************** 
*** Finds the solution of a system of IC equations in IC * * * 
*** unknowns. *** 

*************************************************************** 



REAL*8 A ( 1 ) 

ICP1 = IC + 1 

DO 107 K = 1,IC 

INDEX1 = ( K-l ) * IR + K 

A ( INDEX1 ) = 1 ,D0/A( INDEX1 ) 

DO 102 J = 1,ICP1 
IF(J-K) 3,102,3 
3 INDEX2 = (J-1)*IR + K 

A ( I ND EX2 ) = A( INDEX2)*A( INDEX1 ) 

102 CONTINUE 

DO 115 I = 1,IC 
IF (I-K) 20,115,20 

20 INDEX3 = (K-1)*IR + I 
DO 114 J = 1,ICP1 
IF(J-K) 21,114,21 

21 INDEX2 = (J-1)*IR + I 
INDEX4 = (J - 1)*IR + K 

A( INDEX2) = A ( INDEX2 ) - A( INDEX4 ) *A( INDEX3 ) 

114 CONTINUE 

115 CONTINUE 

DO 107 I = 1,IC 
I F ( I-K ) 22,107,22 

22 INDEX2 = ( K-l ) * I R + I 

A ( I ND EX2 ) = - 1 . DO * A( INDEX2) *A( INDEX1) 

107 CONTINUE 
RETURN 
END 
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SUBROUTINE MMATML ( K , R, L , M, N , B , E , F ) 



************************************************************ 
*** Prepares for a linear transformation by a selected *** 



*** 1th face of B; calls MULMAT to perform the *** 
*** transformation on the Mth row of E and positions *** 
*** the new vector in the Nth row of F. *** 
*** K = number of arrays in the problem *** 
*** R = number of rows in (common to) E and F *** 
*** L = array index; first argument of B *** 
*** M = index of the row of E *** 
*** N = index of the row of F *#* 
*** B = Rank 3 family of rotational matrices *** 
* * * EE=multiplicandvector * * * 
*** EF = product vector *** 



* * * E = input array (of rank 2 ) » row M is used for EE * * * 
*** F = output array (of rank 2),EF is placed in row N *** 
************************************************************ 



INTEGERS I, J, K, l, M, N ,R 

REAL*8 BB(3,3), BOO, 3, 3), EE(3), EF(3), E(30,3), F(30,3) 

... Initialize the temporary variables: 

DO 10 I = 1,3 
EE ( I ) = E ( M » I ) 

DO 10 J = 1,3 

BB ( I , J ) = B ( L , I , J ) 

10 CONTINUE 

... Call routine to perform linear transformation 
CALL MULMAT ( BB , EE , EF ) 

... Position the output as prescribed. 

DO 20 I = 1,3 
F ( N , I ) = E F ( I ) 

20 CONTINUE 

RETURN 

END 



SUBROUTINE DISPLC(K,B,A,IA,DEL,D) 

*** DEL is the set of (output) displacements of the K arrays 
*** estimated by the least square method. 

* * * D is the length of each row of DEL. 

*** B is the set of K reorientation matrices (output of M U L T A R ) . 
*** A is the output of INTCEP (i.e. the array intercept vectors). 

* * * IA is the list of sensor arrays in the problem. 

******** ******** ************ ****************** ****** ************* 
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C ... Declarations. 

C 

REAL*8 B(30,3 >3 ) , A(30,3), AL(30,3), DEL(30,3), AA(3) 

REAL*8 ID (3 >3 ) > BB(3,3), D(30), ARNUM1, ARNUM2, ARNUM3 , T(3) 
INTEGER*4 IA(30), K, DATE 

... Create an identity matrix ID: 

DO 10 1=1,3 

DO 10 J = 1 , 3 

ID ( I , J ) = 0 . DO 

IF ( I .NE. J ) GOTO 9 

ID ( I , J ) = 1 .DO 

9 CONTINUE 

10 CONTINUE 

... Read file AR1.DAT in the order specified by IA. 

This is used to create AL, the matrix of assumed locations 
of those sensor arrays that appear in the problem. 

The identication vector IA produced in CONECT is needed to 
extract the correct rows from AR1 . The result is AL(K,2). 

OPEN (2, FILE= » AR1 .DAT' , STATU S= ’OLD* ) 

J = 1 

17 CONTINUE 

READ(2,*>END=18) IAR, DATE, ARNUM1 , ARNUM2 , ARNUM3 
GOTO 15 

18 CONTINUE 

WRITE(*,*) ' Designated array number not found. ' 

WRITE(*>*) ' Operation aborting. ' 

STOP 

15 CONTINUE 

IF (IAR .EQ. I A ( J ) ) THEN 
AL ( J , 1 ) = ARNUM1 
AL ( J , 2 ) = ARNUM2 
AL ( J , 3 ) = ARNUM3 
J = J + 1 
REWIND (2) 

END I F 

I F ( J .LE. K) GOTO 17 
CLOSE ( UN IT=2 ) 

... Compute the displacements. 

... First reduce by one the diagonal elements of each face of E 
DO 30 KK= 1 , K 
DO 20 1=1,3 
DO 20 J = 1 , 3 

BB ( I , J ) = B ( K K , I , J ) - I D ( I , J ) 

20 CONTINUE 



... Complete the displacement computation: 
DO 22 1=1,3 
22 AA( I) = AL ( KK , I) 

CALL MULM AT ( BB , AA , T ) 



67 



ooo ooo oooooo ooooooooooo ooo 



DO 25 1=1,3 

DEL ( KK » I ) = A(KK,I)+T(I) 

25 CONTINUE 
30 CONTINUE 

... Compute the length of each row of DEL. 

DO 40 KK= 1 » K 
D ( KK ) = 0 . DO 
DO 35 1=1,3 

D ( KK ) = D ( KK ) + ( DEL ( KK , I ) * DEL ( KK , I ) ) 

35 CONTINUE 

40 D ( KK ) = DSQRT ( D ( KK ) ) 

RETURN 

END 



SUBROUTINE MAXANG ( BB , P ) 

REAL*8 B ( 3 , 3 ) , BB(3,3), X(3), Y(3), D, P 

a###****######**###*##****##***#*#******##*##*****#*#**#**###** 
* * * Takes the orthonormal matrix BB as input and constructs * * * 
*** the angle of maximal rotation that any vector can * * * 

*** experience under the transformation BB. *** 

*** Calls MULT3 . *** 

*************************************************************** 



... Begin with the adjustment of the diagonal elements of E 
in order to prepare the eigenvector problem. 

DO 5 1=1,3 
DO 5 J = 1 , 3 
B( I, J ) = BB( I, J ) 

IF (I .EQ. J ) B( I, I) = B( I, I) - 1. 

5 CONTINUE 

... Use the first two equations of the eigenvector system 
(with X3 = 1) to solve for the eigenvector. 

... Compute the determinant of the coefficient matrix: 

D = B(l,l)* 6(2,2) - 6(1,2) *B (2,1) 

... Check for zero rotation and transfer to the end 
if it occurs. 

IF (D .EQ. 0.0) THEN 
D = 1 . DO 
GOTO 30 
END IF 

... Set third component equal to one and solve the linear 
system for the other two. 

X ( 3 ) = 1 . DO 

X(l) = (B(1,2)*B(2»3) - B(1»3)*B(2»2) )/D 
X ( 2 ) = (B(1,3)*B(2>1) - B(l»l)*6(2,3))/D 

... Normalize the eigenvectors. 
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oooooo ooo o ooo oooo 



D = DSQRT ( 1 . DO + X(1)*X(1) + X(2)*X(2)) 
DO 10 1=1,3 
10 X ( I ) = X ( I ) / D 



... Construct a vector orthogonal to the eigenvector. 
First choose the subscript of the smallest X. 



J = 1 

IF ( DABS ( X ( 1 ) ) .GT. DABS(X(2))) J=2 
IF ( D ABS ( X ( J ) ) .GT. DABS (X ( 3 ) ) ) J=3 



15 



20 



... Use the Gram-Schmi dt technique to convert the direction 
of X(J) to a direction orthogonal to X(l), X(2), X ( 3 ) . 
DO 15 1=1,3 
IF (I .EQ. J) THEN 

Y(J) = X(J>*(1.- X(J)*X(J)) 

ELSE 

Y ( I ) = (-1. )*X(I)*X(J)*X(J) 

END IF 
CONTINUE 

... Normalize the new vector. 

D = DSQRT ( Y ( 1 ) *Y ( 1 ) + Y(2)*Y(2) + Y(3)*Y(3) ) 

DO 20 1=1,3 
Y ( I ) = Y(I)/D 



... Transform Y by BB and compute the angle between Y and BB*Y 

CALL MULMAT ( BB , Y , X ) 

D = 0 . DO 

DO 25 1=1,3 

D = D + X(I)*Y(I) 

25 CONTINUE 

30 P = DACOS(D) 

RETURN 

END 



SUBROUTINE ANGLE S ( K , B , P ) 

*********************************** **x*xx* ****** ****xx*x* 
*** Computes the three Euler angles for each of the * * * 
*** K matrices in B. * * * 

******************* ******************** ****** ********* XX* 



REAL*8 B(30»3»3),P(30»3) 

C 

DO 20 KK= 1 , K 

P(KK,2)=DASIN ( B ( KK , 3 , 1 ) ) 

P(KK,1)=DASIN ( B ( KK , 3 , 2 ) /DCCS (P(KK,2))) 
P(KK,3)=DASIN (B(KK,2, D/DCOS (P(KK,2))) 
20 CONTINUE 

RETURN 
END 
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